pprz_algebra_float.h 44.9 KB
Newer Older
1
/*
Felix Ruess's avatar
Felix Ruess committed
2
 * Copyright (C) 2008-2014 The Paparazzi Team
3 4 5 6 7 8 9 10 11 12 13 14 15 16
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
17 18 19 20 21 22
 * along with paparazzi; see the file COPYING.  If not, see
 * <http://www.gnu.org/licenses/>.
 */

/**
 * @file pprz_algebra_float.h
Felix Ruess's avatar
Felix Ruess committed
23
 * @brief Paparazzi floating point algebra.
24
 *
Felix Ruess's avatar
Felix Ruess committed
25 26 27 28
 * @addtogroup math_algebra
 * @{
 * @defgroup math_algebra_float Float Algebra
 * @{
29 30
 */

Antoine Drouin's avatar
Antoine Drouin committed
31 32 33
#ifndef PPRZ_ALGEBRA_FLOAT_H
#define PPRZ_ALGEBRA_FLOAT_H

Felix Ruess's avatar
Felix Ruess committed
34 35 36 37
#ifdef __cplusplus
extern "C" {
#endif

38
#include "pprz_algebra.h"
39
#include "message_pragmas.h"
40

41
#include <math.h>
42
#include <float.h> // for FLT_MIN
43

44 45
#include "pprz_algebra_float_frama_c.h"

Antoine Drouin's avatar
Antoine Drouin committed
46 47 48 49 50
/* this seems to be missing for some arch */
#ifndef M_SQRT2
#define M_SQRT2         1.41421356237309504880
#endif

POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
51 52 53 54
/*******************************
 *         FloatVect2
 * *****************************/

55
struct FloatVect2 {
Antoine Drouin's avatar
Antoine Drouin committed
56 57
  float x;
  float y;
58
};
Antoine Drouin's avatar
Antoine Drouin committed
59

POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76
/*@
  predicate finite_FloatVect2(struct FloatVect2 *v) =
   \is_finite(v->x)
   && \is_finite(v->y);
*/

/*@
  predicate rvalid_FloatVect2(struct FloatVect2 *v) =
   \valid_read(v) && finite_FloatVect2(v);
*/

/*@
  predicate valid_FloatVect2(struct FloatVect2 *v) =
   \valid(v) && finite_FloatVect2(v);
*/


77 78 79 80
/*******************************
 *         FloatVect3
 * *****************************/

Antoine Drouin's avatar
Z  
Antoine Drouin committed
81
struct FloatVect3 {
Antoine Drouin's avatar
Antoine Drouin committed
82 83 84
  float x;
  float y;
  float z;
Antoine Drouin's avatar
Z  
Antoine Drouin committed
85
};
Antoine Drouin's avatar
Antoine Drouin committed
86

87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107
/*@
  predicate finite_FloatVect3(struct FloatVect3 *v) =
   \is_finite(v->x)
   && \is_finite(v->y)
   && \is_finite(v->z);
*/

/*@
  predicate rvalid_FloatVect3(struct FloatVect3 *v) =
   \valid_read(v) && finite_FloatVect3(v);
*/

/*@
  predicate valid_FloatVect3(struct FloatVect3 *v) =
   \valid(v) && finite_FloatVect3(v);
*/

/*******************************
 *          FloatQuat
 * *****************************/

108 109 110
/**
 * @brief Roation quaternion
 */
111
struct FloatQuat {
Antoine Drouin's avatar
Antoine Drouin committed
112 113 114 115
  float qi;
  float qx;
  float qy;
  float qz;
116 117
};

Antoine Drouin's avatar
Antoine Drouin committed
118
struct FloatMat33 {
Felix Ruess's avatar
Felix Ruess committed
119
  float m[3 * 3];
Antoine Drouin's avatar
Antoine Drouin committed
120 121
};

122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139
/*@
  predicate finite_FloatQuat(struct FloatQuat *q) =
   \is_finite(q->qi)
   && \is_finite(q->qx)
   && \is_finite(q->qy)
   && \is_finite(q->qz);
*/

/*@
  predicate rvalid_FloatQuat(struct FloatQuat *q) =
   \valid_read(q) && finite_FloatQuat(q);
*/

/*@
  predicate valid_FloatQuat(struct FloatQuat *q) =
   \valid(q) && finite_FloatQuat(q);
*/

POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
140 141 142 143 144 145 146 147
/*@
  predicate valid_subterms_FloatQuat(struct FloatQuat *q) =
    \valid(&q->qi)
    && \valid(&q->qx)
    && \valid(&q->qy)
    && \valid(&q->qz);
*/

148 149 150 151
/*******************************
 *         FloatRMat
 * *****************************/

152 153 154
/**
 * @brief rotation matrix
 */
Antoine Drouin's avatar
Antoine Drouin committed
155
struct FloatRMat {
Felix Ruess's avatar
Felix Ruess committed
156
  float m[3 * 3];
Antoine Drouin's avatar
Antoine Drouin committed
157 158
};

159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183
/*@
  predicate finite_FloatRMat(struct FloatRMat *rm) =
   \is_finite(rm->m[0])
   && \is_finite(rm->m[1])
   && \is_finite(rm->m[2])
   && \is_finite(rm->m[3])
   && \is_finite(rm->m[4])
   && \is_finite(rm->m[5])
   && \is_finite(rm->m[6])
   && \is_finite(rm->m[7])
   && \is_finite(rm->m[8]);
*/

/*@
  predicate rvalid_FloatRMat(struct FloatRMat *rm) =
   \valid_read(rm) && finite_FloatRMat(rm);
*/

/*@
  predicate valid_FloatRMat(struct FloatRMat *rm) =
   \valid(rm) && finite_FloatRMat(rm);
*/


/*******************************
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
184
 *         FloatEulers
185 186 187
 * *****************************/


188 189 190
/**
 * @brief euler angles
 * @details Units: radians */
191
struct FloatEulers {
192 193 194
  float phi; ///< in radians
  float theta; ///< in radians
  float psi; ///< in radians
195 196
};

197
/*@
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
198
  predicate finite_FloatEulers(struct FloatEulers *e) =
199 200 201 202 203 204
   \is_finite(e->phi)
   && \is_finite(e->theta)
   && \is_finite(e->psi);
*/

/*@
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
205 206 207 208 209 210 211
  predicate rvalid_FloatEulers(struct FloatEulers *e) =
   \valid_read(e) && finite_FloatEulers(e);
*/

/*@
  predicate valid_FloatEulers(struct FloatEulers *e) =
   \valid(e) && finite_FloatEulers(e);
212 213
*/

214 215
/**
 * @brief angular rates
Felix Ruess's avatar
Felix Ruess committed
216
 * @details Units: rad/s */
217
struct FloatRates {
Felix Ruess's avatar
Felix Ruess committed
218 219 220
  float p; ///< in rad/s
  float q; ///< in rad/s
  float r; ///< in rad/s
221
};
Antoine Drouin's avatar
Antoine Drouin committed
222

223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239
/*@
  predicate finite_FloatRates(struct FloatRates *r) =
   \is_finite(r->p)
   && \is_finite(r->q)
   && \is_finite(r->r);
*/

/*@
  predicate rvalid_FloatRates(struct FloatRates *r) =
   \valid_read(r) && finite_FloatRates(r);
*/

/*@
  predicate valid_FloatRates(struct FloatRates *r) =
   \valid(r) && finite_FloatRates(r);
*/

Felix Ruess's avatar
Felix Ruess committed
240 241 242
#define FLOAT_ANGLE_NORMALIZE(_a) {             \
    while (_a >  M_PI) _a -= (2.*M_PI);             \
    while (_a < -M_PI) _a += (2.*M_PI);             \
243 244
  }

245 246
#include "pprz_algebra_float_convert_rmat_frama_c.h"

247 248 249
/*
 * Returns the real part of the log of v in base of n
 */
250 251 252 253
/*@
  requires n > 0;
  assigns \nothing;
*/
254 255 256 257 258 259 260 261 262 263 264
static inline float float_log_n(float v, float n)
{
  if (fabsf(v) < 1e-4) { // avoid inf
    return - 1.0E+30;
  }
  if (fabsf(n) < 1e-4) { // avoid nan
    return 0;
  }
  return logf(fabsf(v)) / logf(n);
}

265 266 267 268 269 270
//
//
// Vector algebra
//
//

271

Felix Ruess's avatar
Felix Ruess committed
272 273 274 275 276 277
/*
 * Dimension 2 Vectors
 */

#define FLOAT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0., 0.)

Felix Ruess's avatar
Felix Ruess committed
278 279 280
/* macros also usable if _v is not a FloatVect2, but a different struct with x,y members */
#define FLOAT_VECT2_NORM(_v) sqrtf(VECT2_NORM2(_v))

281 282 283 284 285
/*@
  requires \valid_read(v);
  assigns \nothing;
  ensures \result >= 0.0;
*/
286
static inline float float_vect2_norm2(struct FloatVect2 *v)
Felix Ruess's avatar
Felix Ruess committed
287 288 289 290
{
  return v->x * v->x + v->y * v->y;
}

291 292 293 294 295
/*@
  requires \valid_read(v);
  assigns \nothing;
  ensures \result >= 0.0;
*/
296
static inline float float_vect2_norm(struct FloatVect2 *v)
Felix Ruess's avatar
Felix Ruess committed
297 298 299 300 301
{
  return sqrtf(float_vect2_norm2(v));
}

/** normalize 2D vector in place */
302 303 304 305
/*@
  requires \valid(v);
  assigns *v;
*/
306
static inline void float_vect2_normalize(struct FloatVect2 *v)
Felix Ruess's avatar
Felix Ruess committed
307 308 309 310 311
{
  const float n = float_vect2_norm(v);
  if (n > 0) {
    v->x /= n;
    v->y /= n;
Felix Ruess's avatar
Felix Ruess committed
312
  }
Felix Ruess's avatar
Felix Ruess committed
313
}
Felix Ruess's avatar
Felix Ruess committed
314

Felix Ruess's avatar
Felix Ruess committed
315
#define FLOAT_VECT2_NORMALIZE(_v) float_vect2_normalize(&(_v))
316

Felix Ruess's avatar
Felix Ruess committed
317

318 319 320
/*
 * Dimension 3 Vectors
 */
Antoine Drouin's avatar
Z  
Antoine Drouin committed
321

Antoine Drouin's avatar
Antoine Drouin committed
322
#define FLOAT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0., 0., 0.)
Antoine Drouin's avatar
Antoine Drouin committed
323

Felix Ruess's avatar
Felix Ruess committed
324 325 326
/* macros also usable if _v is not a FloatVect3, but a different struct with x,y,z members */
#define FLOAT_VECT3_NORM(_v) sqrtf(VECT3_NORM2(_v))

327 328 329 330 331
/*@
  requires \valid_read(v);
  assigns \nothing;
  ensures \result >= 0.0;
*/
332
static inline float float_vect3_norm2(struct FloatVect3 *v)
Felix Ruess's avatar
Felix Ruess committed
333 334 335 336
{
  return v->x * v->x + v->y * v->y + v->z * v->z;
}

337 338 339 340 341
/*@
  requires \valid_read(v);
  assigns \nothing;
  ensures \result >= 0.0;
*/
342
static inline float float_vect3_norm(struct FloatVect3 *v)
Felix Ruess's avatar
Felix Ruess committed
343 344 345 346 347
{
  return sqrtf(float_vect3_norm2(v));
}

/** normalize 3D vector in place */
348 349 350 351
/*@
  requires \valid(v);
  assigns *v;
*/
352
static inline void float_vect3_normalize(struct FloatVect3 *v)
Felix Ruess's avatar
Felix Ruess committed
353 354 355 356 357 358 359 360 361
{
  const float n = float_vect3_norm(v);
  if (n > 0) {
    v->x /= n;
    v->y /= n;
    v->z /= n;
  }
}

362
#define FLOAT_VECT3_NORMALIZE(_v) float_vect3_normalize(&(_v))
363

Antoine Drouin's avatar
Antoine Drouin committed
364

365

Felix Ruess's avatar
Felix Ruess committed
366 367
#define FLOAT_RATES_ZERO(_r) {          \
    RATES_ASSIGN(_r, 0., 0., 0.);       \
Antoine Drouin's avatar
Antoine Drouin committed
368 369
  }

Antoine Drouin's avatar
Antoine Drouin committed
370 371
#define FLOAT_RATES_NORM(_v) (sqrtf((_v).p*(_v).p + (_v).q*(_v).q + (_v).r*(_v).r))

Felix Ruess's avatar
Felix Ruess committed
372 373 374 375
#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2) {          \
    _ro.p = _s1 * _r1.p + _s2 * _r2.p;                  \
    _ro.q = _s1 * _r1.q + _s2 * _r2.q;                  \
    _ro.r = _s1 * _r1.r + _s2 * _r2.r;                  \
376 377
  }

378 379 380 381 382
/*@
  requires valid_FloatVect3(vec);
  requires rvalid_FloatVect3(dv);
  assigns *vec;
*/
383
extern void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv,
384
                                     float dt);
385 386 387 388 389
/*@
  requires valid_FloatRates(r);
  requires rvalid_FloatRates(dr);
  assigns *r;
*/
390
extern void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr,
391
                                     float dt);
392 393
/*@
  requires valid_FloatRates(r);
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
394 395
  requires rvalid_FloatEulers(e);
  requires rvalid_FloatEulers(edot);
396 397
  assigns *r;
*/
398 399
extern void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e,
                                     struct FloatEulers *edot);
400

401
/* defines for backwards compatibility */
402 403 404
#define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt) WARNING("FLOAT_VECT3_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_vect3_integrate_fi(&(_vo), &(_dv), _dt)
#define FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt) WARNING("FLOAT_RATES_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_rates_integrate_fi(&(_ra), &(_racc), _dt)
#define FLOAT_RATES_OF_EULER_DOT(_ra, _e, _ed)  WARNING("FLOAT_RATES_OF_EULER_DOT macro is deprecated, use the lower case function instead") float_rates_of_euler_dot(&(_ra), &(_e), &(_ed))
405 406


Antoine Drouin's avatar
Antoine Drouin committed
407 408 409
/*
 * 3x3 matrices
 */
Felix Ruess's avatar
Felix Ruess committed
410 411 412
#define FLOAT_MAT33_ZERO(_m) {                      \
    MAT33_ELMT(_m, 0, 0) = 0.;                      \
    MAT33_ELMT(_m, 0, 1) = 0.;                      \
413
    MAT33_ELMT(_m, 0, 2) = 0.;                      \
Felix Ruess's avatar
Felix Ruess committed
414 415 416 417 418 419
    MAT33_ELMT(_m, 1, 0) = 0.;                      \
    MAT33_ELMT(_m, 1, 1) = 0.;                      \
    MAT33_ELMT(_m, 1, 2) = 0.;                      \
    MAT33_ELMT(_m, 2, 0) = 0.;                      \
    MAT33_ELMT(_m, 2, 1) = 0.;                      \
    MAT33_ELMT(_m, 2, 2) = 0.;                      \
Antoine Drouin's avatar
Antoine Drouin committed
420 421
  }

422
#define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22) {    \
Felix Ruess's avatar
Felix Ruess committed
423 424 425 426 427 428 429 430 431
    MAT33_ELMT(_m, 0, 0) = _d00;                    \
    MAT33_ELMT(_m, 0, 1) = 0.;                      \
    MAT33_ELMT(_m, 0, 2) = 0.;                      \
    MAT33_ELMT(_m, 1, 0) = 0.;                      \
    MAT33_ELMT(_m, 1, 1) = _d11;                    \
    MAT33_ELMT(_m, 1, 2) = 0.;                      \
    MAT33_ELMT(_m, 2, 0) = 0.;                      \
    MAT33_ELMT(_m, 2, 1) = 0.;                      \
    MAT33_ELMT(_m, 2, 2) = _d22;                    \
Antoine Drouin's avatar
Antoine Drouin committed
432 433 434
  }


435 436 437 438 439 440
//
//
// Rotation Matrices
//
//

Antoine Drouin's avatar
Antoine Drouin committed
441

442
/** initialises a rotation matrix to identity */
443 444 445 446
/*@
  requires \valid(rm);
  assigns *rm;
*/
447
static inline void float_rmat_identity(struct FloatRMat *rm)
448 449 450
{
  FLOAT_MAT33_DIAG(*rm, 1., 1., 1.);
}
Antoine Drouin's avatar
Antoine Drouin committed
451

Felix Ruess's avatar
Felix Ruess committed
452 453 454
/** Inverse/transpose of a rotation matrix.
 * m_b2a = inv(_m_a2b) = transp(_m_a2b)
 */
455 456 457 458 459
/*@
  requires \valid(m_b2a);
  requires rvalid_FloatRMat(m_a2b);
  assigns *m_b2a;
*/
460
extern void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b);
Felix Ruess's avatar
Felix Ruess committed
461 462 463 464

/** Composition (multiplication) of two rotation matrices.
 * m_a2c = m_a2b comp m_b2c , aka  m_a2c = m_b2c * m_a2b
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
465 466 467 468 469 470 471
/*@
  requires valid_FloatRMat(m_a2c);
  requires rvalid_FloatRMat(m_a2b);
  requires rvalid_FloatRMat(m_b2c);
  requires \separated(m_a2c + (0..8), m_a2b + (0..8), m_b2c + (0..8));
  assigns m_a2c[0..8];
*/
472 473
extern void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b,
                            struct FloatRMat *m_b2c);
Felix Ruess's avatar
Felix Ruess committed
474 475 476 477

/** Composition (multiplication) of two rotation matrices.
 * m_a2b = m_a2c comp_inv m_b2c , aka  m_a2b = inv(_m_b2c) * m_a2c
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
478 479 480 481 482 483
/*@
  requires valid_FloatRMat(m_a2b);
  requires rvalid_FloatRMat(m_a2c);
  requires rvalid_FloatRMat(m_b2c);
  assigns m_a2b[0..8];
*/
484 485
extern void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c,
                                struct FloatRMat *m_b2c);
Felix Ruess's avatar
Felix Ruess committed
486 487

/// Norm of a rotation matrix.
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
488 489 490 491
/*@
  requires rvalid_FloatRMat(rm);
  assigns \nothing;
*/
492
extern float float_rmat_norm(struct FloatRMat *rm);
493

Felix Ruess's avatar
Felix Ruess committed
494 495 496
/** rotate 3D vector by rotation matrix.
 * vb = m_a2b * va
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
497 498 499 500 501 502
/*@
  requires valid_FloatVect3(vb);
  requires rvalid_FloatRMat(m_a2b);
  requires rvalid_FloatVect3(va);
  assigns *vb;
*/
503 504
extern void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b,
                             struct FloatVect3 *va);
Felix Ruess's avatar
Felix Ruess committed
505 506 507 508

/** rotate 3D vector by transposed rotation matrix.
 * vb = m_b2a^T * va
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
509 510 511 512 513 514
/*@
  requires valid_FloatVect3(vb);
  requires rvalid_FloatRMat(m_b2a);
  requires rvalid_FloatVect3(va);
  assigns *vb;
*/
515 516
extern void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a,
                                    struct FloatVect3 *va);
Felix Ruess's avatar
Felix Ruess committed
517

518 519 520
/** rotate angle by rotation matrix.
 * rb = m_a2b * ra
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
521 522 523 524 525 526
/*@
  requires valid_FloatEulers(rb);
  requires rvalid_FloatRMat(m_a2b);
  requires rvalid_FloatEulers(ra);
  assigns *rb;
*/
527
extern void float_rmat_mult(struct FloatEulers *rb, struct FloatRMat *m_a2b,
528
                            struct FloatEulers *ra);
529 530 531 532

/** rotate angle by transposed rotation matrix.
 * rb = m_b2a^T * ra
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
533 534 535 536 537 538
/*@
  requires valid_FloatEulers(rb);
  requires rvalid_FloatRMat(m_b2a);
  requires rvalid_FloatEulers(ra);
  assigns *rb;
*/
539
extern void float_rmat_transp_mult(struct FloatEulers *rb, struct FloatRMat *m_b2a,
540
                                   struct FloatEulers *ra);
541

Felix Ruess's avatar
Felix Ruess committed
542 543 544
/** rotate anglular rates by rotation matrix.
 * rb = m_a2b * ra
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
545 546 547 548 549 550
/*@
  requires valid_FloatRates(rb);
  requires rvalid_FloatRMat(m_a2b);
  requires rvalid_FloatRates(ra);
  assigns *rb;
*/
551 552
extern void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b,
                                struct FloatRates *ra);
Felix Ruess's avatar
Felix Ruess committed
553 554 555 556

/** rotate anglular rates by transposed rotation matrix.
 * rb = m_b2a^T * ra
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
557 558 559 560 561 562
/*@
  requires valid_FloatRates(rb);
  requires rvalid_FloatRMat(m_b2a);
  requires rvalid_FloatRates(ra);
  assigns *rb;
*/
563 564
extern void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a,
                                       struct FloatRates *ra);
Felix Ruess's avatar
Felix Ruess committed
565

566
/** initialises a rotation matrix from unit vector axis and angle */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
567 568 569 570 571
/*@
  requires valid_FloatRMat(rm);
  requires rvalid_FloatVect3(uv);
  assigns *rm;
*/
572
extern void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle);
Felix Ruess's avatar
Felix Ruess committed
573

574 575 576 577 578 579 580 581 582 583 584 585
/** Rotation matrix from 321 Euler angles (float).
 * The Euler angles are interpreted as zy'x'' (intrinsic) rotation.
 * First rotate around z with psi, then around the new y' with theta,
 * then around new x'' with phi.
 * This is the same as a xyz (extrinsic) rotation,
 * rotating around the fixed x, then y then z axis.
 * - psi range: -pi < psi <= pi
 * - theta range: -pi/2 <= theta <= pi/2
 * - phi range: -pi < phi <= pi
 * @param[out] rm pointer to rotation matrix
 * @param[in]  e pointer to Euler angles
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
586 587 588
/*@
  requires valid_FloatRMat(rm);
  requires rvalid_FloatEulers(e);
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
589
  ensures rotation_matrix(l_RMat_of_FloatRMat(rm));
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
590 591
  assigns *rm;
*/
592
extern void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e);
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
593 594 595
/*@
  requires valid_FloatRMat(rm);
  requires rvalid_FloatEulers(e);
596
  ensures rotation_matrix(l_RMat_of_FloatRMat(rm));
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
597 598
  assigns *rm;
*/
599
extern void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e);
Felix Ruess's avatar
Felix Ruess committed
600
#define float_rmat_of_eulers float_rmat_of_eulers_321
Felix Ruess's avatar
Felix Ruess committed
601

POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
602 603 604
/*@
  requires valid_FloatRMat(rm);
  requires rvalid_FloatQuat(q);
605 606 607 608
  requires unary_quaterion(q);
  requires \separated(rm, q);
  ensures transpose(l_RMat_of_FloatRMat(rm)) ==  l_RMat_of_FloatQuat(q);
  ensures rotation_matrix(l_RMat_of_FloatRMat(rm));
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
609 610
  assigns *rm;
*/
611
extern void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q);
612

613
/** in place first order integration of a rotation matrix */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
614 615 616 617 618
/*@
  requires valid_FloatRMat(rm);
  requires rvalid_FloatRates(omega);
  assigns *rm;
*/
619
extern void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt);
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
620 621 622 623
/*@
  requires valid_FloatRMat(rm);
  assigns *rm;
*/
624
extern float float_rmat_reorthogonalize(struct FloatRMat *rm);
625 626

/* defines for backwards compatibility */
627 628 629 630 631 632 633 634 635 636 637 638 639 640
#define FLOAT_RMAT_INV(_m_b2a, _m_a2b) WARNING("FLOAT_RMAT_INV macro is deprecated, use the lower case function instead") float_rmat_inv(&(_m_b2a), &(_m_a2b))
#define FLOAT_RMAT_NORM(_m) WARNING("FLOAT_RMAT_NORM macro is deprecated, use the lower case function instead") float_rmat_norm(&(_m))
#define FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c) WARNING("FLOAT_RMAT_COMP macro is deprecated, use the lower case function instead") float_rmat_comp(&(_m_a2c), &(_m_a2b), &(_m_b2c))
#define FLOAT_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c) WARNING("FLOAT_RMAT_COMP_INV macro is deprecated, use the lower case function instead") float_rmat_comp_inv(&(_m_a2b), &(_m_a2c), &(_m_b2c))
#define FLOAT_RMAT_VMULT(_vb, _m_a2b, _va) WARNING("FLOAT_RMAT_VMULT macro is deprecated, use the lower case function instead") float_rmat_vmult(&(_vb), &(_m_a2b), &(_va))
#define FLOAT_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va) WARNING("FLOAT_RMAT_TRANSP_VMULT macro is deprecated, use the lower case function instead") float_rmat_transp_vmult(&(_vb), &(_m_b2a), &(_va))
#define FLOAT_RMAT_RATEMULT(_rb, _m_a2b, _ra) WARNING("FLOAT_RMAT_RATEMULT macro is deprecated, use the lower case function instead") float_rmat_ratemult(&(_rb), &(_m_a2b), &(_ra))
#define FLOAT_RMAT_TRANSP_RATEMULT(_rb, _m_b2a, _ra) WARNING("FLOAT_RMAT_TRANSP_RATEMULT macro is deprecated, use the lower case function instead") float_rmat_ratemult(&(_rb), &(_m_b2a), &(_ra))
#define FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _uv, _an) WARNING("FLOAT_RMAT_OF_AXIS_ANGLE macro is deprecated, use the lower case function instead") float_rmat_of_axis_angle(&(_rm), &(_uv), _an)
#define FLOAT_RMAT_OF_EULERS(_rm, _e)     WARNING("FLOAT_RMAT_OF_EULERS macro is deprecated, use the lower case function instead") float_rmat_of_eulers_321(&(_rm), &(_e))
#define FLOAT_RMAT_OF_EULERS_321(_rm, _e) WARNING("FLOAT_RMAT_OF_EULERS_321 macro is deprecated, use the lower case function instead") float_rmat_of_eulers_321(&(_rm), &(_e))
#define FLOAT_RMAT_OF_EULERS_312(_rm, _e) WARNING("FLOAT_RMAT_OF_EULERS_312 macro is deprecated, use the lower case function instead") float_rmat_of_eulers_312(&(_rm), &(_e))
#define FLOAT_RMAT_OF_QUAT(_rm, _q)       WARNING("FLOAT_RMAT_OF_QUAT macro is deprecated, use the lower case function instead") float_rmat_of_quat(&(_rm), &(_q))
#define FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt) WARNING("FLOAT_RMAT_INTEGRATE_FI macro is deprecated, use the lower case function instead") float_rmat_integrate_fi(&(_rm), &(_omega), &(_dt))
641 642


Antoine Drouin's avatar
Antoine Drouin committed
643

644 645 646 647 648
//
//
// Quaternion algebras
//
//
649

650
/** initialises a quaternion to identity */
651 652 653 654
/*@
  requires \valid(q);
  assigns *q;
*/
655
static inline void float_quat_identity(struct FloatQuat *q)
656 657 658 659 660 661
{
  q->qi = 1.0;
  q->qx = 0;
  q->qy = 0;
  q->qz = 0;
}
662

663
#define FLOAT_QUAT_NORM2(_q) (SQUARE((_q).qi) + SQUARE((_q).qx) + SQUARE((_q).qy) + SQUARE((_q).qz))
664

665 666 667 668
/*@
  requires \valid_read(q);
  assigns \nothing;
*/
669
static inline float float_quat_norm(struct FloatQuat *q)
Felix Ruess's avatar
Felix Ruess committed
670 671
{
  return sqrtf(SQUARE(q->qi) + SQUARE(q->qx) +  SQUARE(q->qy) + SQUARE(q->qz));
672
}
673

674 675 676 677
/*@
  requires \valid(q);
  assigns *q;
*/
678
static inline void float_quat_normalize(struct FloatQuat *q)
Felix Ruess's avatar
Felix Ruess committed
679
{
680 681 682 683 684 685 686
  float qnorm = float_quat_norm(q);
  if (qnorm > FLT_MIN) {
    q->qi = q->qi / qnorm;
    q->qx = q->qx / qnorm;
    q->qy = q->qy / qnorm;
    q->qz = q->qz / qnorm;
  }
687
}
688

689 690 691 692 693
/*@
  requires \valid(qo);
  requires \valid_read(qi);
  assigns *qo;
*/
694
static inline void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
695 696 697 698
{
  QUAT_INVERT(*qo, *qi);
}

699 700 701 702
/*@
  requires \valid(q);
  assigns *q;
*/
703
static inline void float_quat_wrap_shortest(struct FloatQuat *q)
Felix Ruess's avatar
Felix Ruess committed
704 705 706 707
{
  if (q->qi < 0.) {
    QUAT_EXPLEMENTARY(*q, *q);
  }
708
}
Antoine Drouin's avatar
Antoine Drouin committed
709

710 711 712
#define FLOAT_QUAT_EXTRACT(_vo, _qi) QUAT_EXTRACT_Q(_vo, _qi)


Felix Ruess's avatar
Felix Ruess committed
713 714 715
/** Composition (multiplication) of two quaternions.
 * a2c = a2b comp b2c , aka  a2c = a2b * b2c
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
716 717 718 719 720 721
/*@
  requires valid_FloatQuat(a2c);
  requires rvalid_FloatQuat(a2b);
  requires rvalid_FloatQuat(b2c);
  assigns *a2c;
*/
722
extern void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c);
Antoine Drouin's avatar
Antoine Drouin committed
723

Felix Ruess's avatar
Felix Ruess committed
724 725 726
/** Composition (multiplication) of two quaternions.
 * a2b = a2c comp_inv b2c , aka  a2b = a2c * inv(b2c)
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
727 728 729 730 731 732
/*@
  requires valid_FloatQuat(a2b);
  requires rvalid_FloatQuat(a2c);
  requires rvalid_FloatQuat(b2c);
  assigns *a2b;
*/
733
extern void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c);
Antoine Drouin's avatar
Antoine Drouin committed
734

Felix Ruess's avatar
Felix Ruess committed
735 736 737
/** Composition (multiplication) of two quaternions.
 * b2c = a2b inv_comp a2c , aka  b2c = inv(_a2b) * a2c
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
738 739 740 741 742 743
/*@
  requires valid_FloatQuat(b2c);
  requires rvalid_FloatQuat(a2b);
  requires rvalid_FloatQuat(a2c);
  assigns *b2c;
*/
744
extern void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c);
745

Felix Ruess's avatar
Felix Ruess committed
746 747 748
/** Composition (multiplication) of two quaternions with normalization.
 * a2c = a2b comp b2c , aka  a2c = a2b * b2c
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
749 750 751 752 753 754
/*@
  requires valid_FloatQuat(a2c);
  requires rvalid_FloatQuat(a2b);
  requires rvalid_FloatQuat(b2c);
  assigns *a2c;
*/
755
extern void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c);
756

Felix Ruess's avatar
Felix Ruess committed
757 758 759
/** Composition (multiplication) of two quaternions with normalization.
 * a2b = a2c comp_inv b2c , aka  a2b = a2c * inv(b2c)
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
760 761 762 763 764 765
/*@
  requires valid_FloatQuat(a2b);
  requires rvalid_FloatQuat(a2c);
  requires rvalid_FloatQuat(b2c);
  assigns *a2b;
*/
766
extern void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c);
Antoine Drouin's avatar
Antoine Drouin committed
767

Felix Ruess's avatar
Felix Ruess committed
768 769 770
/** Composition (multiplication) of two quaternions with normalization.
 * b2c = a2b inv_comp a2c , aka  b2c = inv(_a2b) * a2c
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
771 772 773 774 775 776
/*@
  requires valid_FloatQuat(b2c);
  requires rvalid_FloatQuat(a2b);
  requires rvalid_FloatQuat(a2c);
  assigns *b2c;
*/
777
extern void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c);
778

Felix Ruess's avatar
Felix Ruess committed
779 780 781 782 783
/** Quaternion derivative from rotational velocity.
 * qd = -0.5*omega(r) * q
 * or equally:
 * qd = 0.5 * q * omega(r)
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
784 785 786 787 788 789
/*@
  requires valid_FloatQuat(qd);
  requires rvalid_FloatRates(r);
  requires rvalid_FloatQuat(q);
  assigns *qd;
*/
790
extern void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q);
Felix Ruess's avatar
Felix Ruess committed
791 792 793 794 795 796

/** Quaternion derivative from rotational velocity with Lagrange multiplier.
 * qd = -0.5*omega(r) * q
 * or equally:
 * qd = 0.5 * q * omega(r)
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
797 798 799 800 801 802
/*@
  requires valid_FloatQuat(qd);
  requires rvalid_FloatRates(r);
  requires rvalid_FloatQuat(q);
  assigns *qd;
*/
803
extern void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q);
Felix Ruess's avatar
Felix Ruess committed
804 805 806

/** Delta rotation quaternion with constant angular rates.
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
807 808 809 810 811
/*@
  requires valid_FloatQuat(q_out);
  requires rvalid_FloatRates(w);
  assigns *q_out;
*/
812
extern void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt);
813

814
/** in place first order quaternion integration with constant rotational velocity */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
815 816 817 818 819
/*@
  requires valid_FloatQuat(q);
  requires rvalid_FloatRates(omega);
  assigns *q;
*/
820
extern void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt);
821

822
/** in place quaternion integration with constant rotational velocity */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
823 824 825 826 827
/*@
  requires valid_FloatQuat(q);
  requires rvalid_FloatRates(omega);
  assigns *q;
*/
828
extern void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt);
829

Felix Ruess's avatar
Felix Ruess committed
830 831
/** rotate 3D vector by quaternion.
 * vb = q_a2b * va * q_a2b^-1
832
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
833 834 835 836 837 838
/*@
  requires valid_FloatVect3(v_out);
  requires rvalid_FloatQuat(q);
  requires rvalid_FloatVect3(v_in);
  assigns *v_out;
*/
Freek van Tienen's avatar
Freek van Tienen committed
839
extern void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in);
840

Felix Ruess's avatar
Felix Ruess committed
841
/// Quaternion from Euler angles.
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
842 843 844 845 846
/*@
  requires valid_FloatQuat(q);
  requires rvalid_FloatEulers(e);
  assigns *q;
*/
847
extern void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e);
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
848 849 850 851 852
/*@
  requires valid_FloatQuat(q);
  requires rvalid_FloatEulers(e);
  assigns *q;
*/
853
extern void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e);
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
854 855 856 857 858
/*@
  requires valid_FloatQuat(q);
  requires rvalid_FloatEulers(e);
  assigns *q;
*/
859
extern void float_quat_of_eulers_yxz(struct FloatQuat *q, struct FloatEulers *e);
Felix Ruess's avatar
Felix Ruess committed
860

861 862 863 864
/** Quaternion from unit vector and angle.
 * Output quaternion is not normalized.
 * It will be a unit quaternion only if the input vector is also unitary.
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
865 866 867 868 869
/*@
  requires valid_FloatQuat(q);
  requires rvalid_FloatVect3(uv);
  assigns *q;
*/
870
extern void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle);
Felix Ruess's avatar
Felix Ruess committed
871 872 873 874

/** Quaternion from orientation vector.
 * Length/norm of the vector is the angle.
 */
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
875 876 877 878 879
/*@
  requires valid_FloatQuat(q);
  requires rvalid_FloatVect3(ov);
  assigns *q;
*/
880
extern void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov);
Felix Ruess's avatar
Felix Ruess committed
881 882

/// Quaternion from rotation matrix.
883
/*@
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
884 885
  requires valid_FloatQuat(q);
  requires rvalid_FloatRMat(rm);
886 887
  requires rotation_matrix(l_RMat_of_FloatRMat(rm));
  requires \separated(rm, q);
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
888
  ensures unary_quaterion(q);
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
889
  assigns *q;
890 891 892 893 894 895 896 897 898 899 900 901 902

  behavior trace_pos:
    assumes trace(rm) > 0;
    ensures l_FloatQuat_of_RMat_trace_pos_t(rm) == l_Quat_of_FloatQuat(q);

  behavior a00_max:
    assumes trace(rm) <= 0
            && RMAT_ELMT(*rm, 0, 0) > RMAT_ELMT(*rm, 1, 1)
            && RMAT_ELMT(*rm, 0, 0) > RMAT_ELMT(*rm, 2, 2);
    ensures l_FloatQuat_of_RMat_0_max_t(rm) == l_Quat_of_FloatQuat(q);

  behavior a11_max:
    assumes trace(rm) <= 0
903
            && RMAT_ELMT(*rm, 1, 1) >= RMAT_ELMT(*rm, 0, 0)
904 905 906 907 908
            && RMAT_ELMT(*rm, 1, 1) > RMAT_ELMT(*rm, 2, 2);
    ensures l_FloatQuat_of_RMat_1_max_t(rm) == l_Quat_of_FloatQuat(q);

  behavior a22_max:
    assumes trace(rm) <= 0
909 910
            && RMAT_ELMT(*rm, 2, 2) >= RMAT_ELMT(*rm, 0, 0)
            && RMAT_ELMT(*rm, 2, 2) >= RMAT_ELMT(*rm, 1, 1);
911 912 913 914
    ensures l_FloatQuat_of_RMat_2_max_t(rm) == l_Quat_of_FloatQuat(q);

  disjoint behaviors;
  complete behaviors;
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
915
*/
916
extern void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm);
917 918 919


/* defines for backwards compatibility */
920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940
#define FLOAT_QUAT_ZERO(_q) WARNING("FLOAT_QUAT_ZERO macro is deprecated, use the lower case function instead") float_quat_identity(&(_q))
#define FLOAT_QUAT_INVERT(_qo, _qi) WARNING("FLOAT_QUAT_INVERT macro is deprecated, use the lower case function instead") float_quat_invert(&(_qo), &(_qi))
#define FLOAT_QUAT_WRAP_SHORTEST(_q) WARNING("FLOAT_QUAT_WRAP_SHORTEST macro is deprecated, use the lower case function instead") float_quat_wrap_shortest(&(_q))
#define FLOAT_QUAT_NORM(_q) WARNING("FLOAT_QUAT_NORM macro is deprecated, use the lower case function instead") float_quat_norm(&(_q))
#define FLOAT_QUAT_NORMALIZE(_q) WARNING("FLOAT_QUAT_NORMALIZE macro is deprecated, use the lower case function instead") float_quat_normalize(&(_q))
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) WARNING("FLOAT_QUAT_COMP macro is deprecated, use the lower case function instead") float_quat_comp(&(_a2c), &(_a2b), &(_b2c))
#define FLOAT_QUAT_MULT(_a2c, _a2b, _b2c) WARNING("FLOAT_QUAT_MULT macro is deprecated, use the lower case function instead") float_quat_comp(&(_a2c), &(_a2b), &(_b2c))
#define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c) WARNING("FLOAT_QUAT_INV_COMP macro is deprecated, use the lower case function instead") float_quat_inv_comp(&(_b2c), &(_a2b), &(_a2c))
#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c) WARNING("FLOAT_QUAT_COMP_INV macro is deprecated, use the lower case function instead") float_quat_comp_inv(&(_a2b), &(_a2c), &(_b2c))
#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) WARNING("FLOAT_QUAT_COMP_NORM_SHORTEST macro is deprecated, use the lower case function instead") float_quat_comp_norm_shortest(&(_a2c), &(_a2b), &(_b2c))
#define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) WARNING("FLOAT_QUAT_COMP_INV_NORM_SHORTEST macro is deprecated, use the lower case function instead") float_quat_comp_inv_norm_shortest(&(_a2b), &(_a2c), &(_b2c))
#define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) WARNING("FLOAT_QUAT_INV_COMP_NORM_SHORTEST macro is deprecated, use the lower case function instead") float_quat_inv_comp_norm_shortest(&(_b2c), &(_a2b), &(_a2c))
#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) WARNING("FLOAT_QUAT_DIFFERENTIAL macro is deprecated, use the lower case function instead") float_quat_differential(&(q_out), &(w), dt)
#define FLOAT_QUAT_INTEGRATE(_q, _omega, _dt) WARNING("FLOAT_QUAT_INTEGRATE macro is deprecated, use the lower case function instead") float_quat_integrate(&(_q), &(_omega), _dt)
#define FLOAT_QUAT_VMULT(v_out, q, v_in) WARNING("FLOAT_QUAT_VMULT macro is deprecated, use the lower case function instead") float_quat_vmult(&(v_out), &(q), &(v_in))
#define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q) WARNING("FLOAT_QUAT_DERIVATIVE macro is deprecated, use the lower case function instead") float_quat_derivative(&(_qd), &(_r), &(_q))
#define FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q) WARNING("FLOAT_QUAT_DERIVATIVE_LAGRANGE macro is deprecated, use the lower case function instead") float_quat_derivative_lagrange(&(_qd), &(_r), &(_q))
#define FLOAT_QUAT_OF_EULERS(_q, _e) WARNING("FLOAT_QUAT_OF_EULERS macro is deprecated, use the lower case function instead") float_quat_of_eulers(&(_q), &(_e))
#define FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) WARNING("FLOAT_QUAT_OF_AXIS_ANGLE macro is deprecated, use the lower case function instead") float_quat_of_axis_angle(&(_q), &(_uv), _an)
#define FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov) WARNING("FLOAT_QUAT_OF_ORIENTATION_VECT macro is deprecated, use the lower case function instead") float_quat_of_orientation_vect(&(_q), &(_ov))
#define FLOAT_QUAT_OF_RMAT(_q, _r) WARNING("FLOAT_QUAT_OF_RMAT macro is deprecated, use the lower case function instead") float_quat_of_rmat(&(_q), &(_r))
941

Antoine Drouin's avatar
Antoine Drouin committed
942

943 944 945 946 947 948

//
//
// Euler angles
//
//
Antoine Drouin's avatar
Antoine Drouin committed
949 950 951

#define FLOAT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0., 0., 0.);

952
/*@
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
953
  requires rvalid_FloatEulers(e);
954 955
  assigns \nothing;
*/
956
static inline float float_eulers_norm(struct FloatEulers *e)
Felix Ruess's avatar
Felix Ruess committed
957 958
{
  return sqrtf(SQUARE(e->phi) + SQUARE(e->theta) + SQUARE(e->psi));
959
}
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
960 961 962 963 964
/*@
  requires valid_FloatEulers(e);
  requires rvalid_FloatRMat(rm);
  assigns *e;
*/
965
extern void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm);
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
966 967 968 969 970
/*@
  requires valid_FloatEulers(e);
  requires rvalid_FloatQuat(q);
  assigns *e;
*/
971
extern void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q);
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
972 973 974 975 976
/*@
  requires valid_FloatEulers(e);
  requires rvalid_FloatQuat(q);
  assigns *e;
*/
977
extern void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q);
POLLIEN Baptiste's avatar
POLLIEN Baptiste committed
978 979 980 981 982
/*@
  requires valid_FloatEulers(e);
  requires rvalid_FloatQuat(q);
  assigns *e;
*/
983
extern void float_eulers_of_quat_yxz(struct FloatEulers *e, struct FloatQuat *q);
Antoine Drouin's avatar
Antoine Drouin committed
984

985
/* defines for backwards compatibility */
986 987 988
#define FLOAT_EULERS_OF_RMAT(_e, _rm) WARNING("FLOAT_EULERS_OF_RMAT macro is deprecated, use the lower case function instead") float_eulers_of_rmat(&(_e), &(_rm))
#define FLOAT_EULERS_OF_QUAT(_e, _q) WARNING("FLOAT_EULERS_OF_QUAT macro is deprecated, use the lower case function instead") float_eulers_of_quat(&(_e), &(_q))
#define FLOAT_EULERS_NORM(_e) WARNING("FLOAT_EULERS_NORM macro is deprecated, use the lower case function instead") float_eulers_norm(&(_e))
Antoine Drouin's avatar
Antoine Drouin committed
989

990 991 992 993 994 995 996
//
//
// Generic vector algebra
//
//

/** a = 0 */
997 998 999 1000
/*@
  requires valid_float_vect(a, n);
  assigns a[0..n-1];
*/
1001
static inline void float_vect_zero(float *a, const int n)
Felix Ruess's avatar
Felix Ruess committed
1002
{
1003
  int i;
1004 1005 1006 1007 1008
  /*@
    loop invariant 0 <= i <= n;
    loop assigns a[0..n-1], i;
    loop variant n - i;
  */
1009 1010 1011 1012
  for (i = 0; i < n; i++) { a[i] = 0.; }
}

/** a = b */
1013 1014 1015 1016 1017
/*@
  requires valid_float_vect(a, n);
  requires rvalid_float_vect(b, n);
  assigns a[0..n-1];
*/
1018
static inline void float_vect_copy(float *a, const float *b, const int n)
Felix Ruess's avatar
Felix Ruess committed
1019
{
1020
  int i;
1021 1022 1023 1024 1025
  /*@
    loop invariant 0 <= i <= n;
    loop assigns a[0..n-1], i;
    loop variant n - i;
  */
1026 1027 1028 1029
  for (i = 0; i < n; i++) { a[i] = b[i]; }
}

/** o = a + b */
1030 1031 1032 1033 1034 1035
/*@
  requires valid_float_vect(o, n);
  requires rvalid_float_vect(a, n);
  requires rvalid_float_vect(b, n);
  assigns o[0..n-1];
*/
1036
static inline void float_vect_sum(float *o, const float *a, const float *b, const int n)
Felix Ruess's avatar
Felix Ruess committed
1037
{
1038
  int i;
1039 1040 1041 1042 1043
  /*@
    loop invariant 0 <= i <= n;
    loop assigns o[0..n-1], i;
    loop variant n - i;
  */
1044 1045 1046 1047
  for (i = 0; i < n; i++) { o[i] = a[i] + b[i]; }
}

/** o = a - b */
1048 1049 1050 1051 1052 1053
/*@
  requires valid_float_vect(o, n);
  requires rvalid_float_vect(a, n);
  requires rvalid_float_vect(b, n);
  assigns o[0..n-1];
*/
1054
static inline void float_vect_diff(float *o, const float *a, const float *b, const int n)
Felix Ruess's avatar
Felix Ruess committed
1055
{
1056
  int i;
1057 1058 1059 1060 1061
  /*@
    loop invariant 0 <= i <= n;
    loop assigns o[0..n-1], i;
    loop variant n - i;
  */
1062 1063 1064 1065
  for (i = 0; i < n; i++) { o[i] = a[i] - b[i]; }
}

/** o = a * b (element wise) */