Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-21 03:43:26 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[python] nicer wrappers for float and double algebra

Browse Source
This commit is contained in:
Felix Ruess
2015-01-10 15:21:00 +01:00
parent 0e529758c0
commit f044d7d1f0
2 changed files with 426 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/lib/python/pprz_math/pprz_algebra_double.i
+174
View File
@@ -4,4 +4,178 @@
#include "math/pprz_algebra_double.h"
%}
/* don't wrap everything in header
%include "math/pprz_algebra_double.h"
* instead only wrap the structs and extend them with nicer to use methods
*/
struct DoubleVect2 {
double x;
double y;
};
struct DoubleVect3 {
double x;
double y;
double z;
};
struct DoubleQuat {
double qi;
double qx;
double qy;
double qz;
};
struct DoubleRMat {
double m[3 * 3];
};
struct DoubleEulers {
double phi; ///< in radians
double theta; ///< in radians
double psi; ///< in radians
};
%extend DoubleVect2 {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Vect2(%g, %g)", $self->x ,$self->y);
return tmp;
}
DoubleVect2(double x=0.0, double y=0.0) {
struct DoubleVect2 *v = (struct DoubleVect2 *) malloc(sizeof(struct DoubleVect2));
v->x = x;
v->y = y;
return v;
}
struct DoubleVect2 __add__(struct DoubleVect2 *other) {
struct DoubleVect2 v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
return v;
}
struct DoubleVect2 __sub__(struct DoubleVect2 *other) {
struct DoubleVect2 v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
return v;
}
double norm() {
return sqrt($self->x*$self->x + $self->y*$self->y);
}
void normalize() {
const double n = sqrt($self->x*$self->x + $self->y*$self->y);
if (n > 0) {
$self->x /= n;
$self->y /= n;
}
}
};
%extend DoubleVect3 {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Vect3(%g, %g, %g)", $self->x ,$self->y, $self->z);
return tmp;
}
DoubleVect3(double x=0.0, double y=0.0, double z=0.0) {
struct DoubleVect3 *v = (struct DoubleVect3 *) malloc(sizeof(struct DoubleVect3));
v->x = x;
v->y = y;
v->z = z;
return v;
}
struct DoubleVect3 __add__(struct DoubleVect3 *other) {
struct DoubleVect3 v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
v.z = $self->z + other->z;
return v;
}
struct DoubleVect3 __sub__(struct DoubleVect3 *other) {
struct DoubleVect3 v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
v.z = $self->z - other->z;
return v;
}
struct DoubleVect3 __rmul__(struct DoubleQuat *q) {
struct DoubleVect3 v;
double_quat_vmult(&v, q, $self);
return v;
}
double norm() {
return double_vect3_norm($self);
}
void normalize() {
double_vect3_normalize($self);
}
};
%extend DoubleQuat {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Quat(%g, %g, %g, %g)", $self->qi, $self->qx ,$self->qy, $self->qz);
return tmp;
}
DoubleQuat(double qi=1.0, double qx=0.0, double qy=0.0, double qz=0.0) {
struct DoubleQuat *v = (struct DoubleQuat *) malloc(sizeof(struct DoubleQuat));
v->qi = qi;
v->qx = qx;
v->qy = qy;
v->qz = qz;
return v;
}
double norm() {
return double_quat_norm($self);
}
void normalize() {
double_quat_normalize($self);
}
void set_identity() {
double_quat_identity($self);
}
struct DoubleEulers to_eulers() {
struct DoubleEulers e;
double_eulers_of_quat(&e, $self);
return e;
}
};
%extend DoubleQuat {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"RMat[% .5f, % .5f, % .5f]\n [% .5f, % .5f, % .5f]\n [% .5f, % .5f, % .5f]",
$self->m[0], $self->m[1], $self->m[2], $self->m[3], $self->m[4],
$self->m[5], $self->m[6], $self->m[7], $self->m[8]);
return tmp;
}
};
%extend DoubleEulers {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Eulers in deg (phi=%g, theta=%g, psi=%g)", DegOfRad($self->phi),
DegOfRad($self->theta), DegOfRad($self->psi));
return tmp;
}
DoubleEulers(double phi=0.0, double theta=0.0, double psi=0.0) {
struct DoubleEulers *v = (struct DoubleEulers *) malloc(sizeof(struct DoubleEulers));
v->phi = phi;
v->theta = theta;
v->psi = psi;
return v;
}
struct DoubleQuat to_quat() {
struct DoubleQuat q;
double_quat_of_eulers(&q, $self);
return q;
}
struct DoubleRMat to_rmat() {
struct DoubleRMat r;
double_rmat_of_eulers(&r, $self);
return r;
}
};
sw/lib/python/pprz_math/pprz_algebra_float.i
+252
View File
@@ -4,4 +4,256 @@
#include "math/pprz_algebra_float.h"
%}
/* don't wrap everything in header
%include "math/pprz_algebra_float.h"
* instead only wrap the structs and extend them with nicer to use methods
*/
struct FloatVect2 {
float x;
float y;
};
struct FloatVect3 {
float x;
float y;
float z;
};
struct FloatQuat {
float qi;
float qx;
float qy;
float qz;
};
struct FloatRMat {
float m[3 * 3];
};
struct FloatEulers {
float phi; ///< in radians
float theta; ///< in radians
float psi; ///< in radians
};
%extend FloatVect2 {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Vect2(%g, %g)", $self->x ,$self->y);
return tmp;
}
FloatVect2(float x=0.0, float y=0.0) {
struct FloatVect2 *v = (struct FloatVect2 *) malloc(sizeof(struct FloatVect2));
v->x = x;
v->y = y;
return v;
}
struct FloatVect2 __add__(struct FloatVect2 *other) {
struct FloatVect2 v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
return v;
}
struct FloatVect2 __sub__(struct FloatVect2 *other) {
struct FloatVect2 v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
return v;
}
float norm2() {
return float_vect2_norm2($self);
}
float norm() {
return float_vect2_norm($self);
}
void normalize() {
float_vect2_normalize($self);
}
};
%extend FloatVect3 {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Vect3(%g, %g, %g)", $self->x ,$self->y, $self->z);
return tmp;
}
FloatVect3(float x=0.0, float y=0.0, float z=0.0) {
struct FloatVect3 *v = (struct FloatVect3 *) malloc(sizeof(struct FloatVect3));
v->x = x;
v->y = y;
v->z = z;
return v;
}
struct FloatVect3 __add__(struct FloatVect3 *other) {
struct FloatVect3 v;
v.x = $self->x + other->x;
v.y = $self->y + other->y;
v.z = $self->z + other->z;
return v;
}
struct FloatVect3 __sub__(struct FloatVect3 *other) {
struct FloatVect3 v;
v.x = $self->x - other->x;
v.y = $self->y - other->y;
v.z = $self->z - other->z;
return v;
}
struct FloatVect3 __rmul__(struct FloatQuat *q) {
struct FloatVect3 v;
float_quat_vmult(&v, q, $self);
return v;
}
float norm2() {
return float_vect3_norm2($self);
}
float norm() {
return float_vect3_norm($self);
}
void normalize() {
float_vect3_normalize($self);
}
};
%extend FloatQuat {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Quat(%g, %g, %g, %g)", $self->qi, $self->qx ,$self->qy, $self->qz);
return tmp;
}
FloatQuat(float qi=1.0, float qx=0.0, float qy=0.0, float qz=0.0) {
struct FloatQuat *v = (struct FloatQuat *) malloc(sizeof(struct FloatQuat));
v->qi = qi;
v->qx = qx;
v->qy = qy;
v->qz = qz;
return v;
}
struct FloatQuat __mul__(struct FloatQuat *q) {
struct FloatQuat qout;
float_quat_comp(&qout, $self, q);
return qout;
}
struct FloatQuat __mul__(struct FloatRMat *rm) {
struct FloatQuat q;
float_quat_of_rmat(&q, rm);
struct FloatQuat qout;
float_quat_comp(&qout, $self, &q);
return qout;
}
float norm() {
return float_quat_norm($self);
}
void normalize() {
float_quat_normalize($self);
}
void set_identity() {
float_quat_identity($self);
}
void wrap_shortest() {
float_quat_wrap_shortest($self);
}
struct FloatEulers to_eulers() {
struct FloatEulers e;
float_eulers_of_quat(&e, $self);
return e;
}
struct FloatRMat to_rmat() {
struct FloatRMat rm;
float_rmat_of_quat(&rm, $self);
return rm;
}
};
%extend FloatRMat {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"RMat[% .5f, % .5f, % .5f]\n [% .5f, % .5f, % .5f]\n [% .5f, % .5f, % .5f]",
$self->m[0], $self->m[1], $self->m[2], $self->m[3], $self->m[4],
$self->m[5], $self->m[6], $self->m[7], $self->m[8]);
return tmp;
}
FloatRMat() {
struct FloatRMat *rm = (struct FloatRMat *) malloc(sizeof(struct FloatRMat));
float_rmat_identity(rm);
return rm;
}
struct FloatRMat __mul__(struct FloatRMat *m_b2c) {
struct FloatRMat m_a2c;
float_rmat_comp(&m_a2c, $self, m_b2c);
return m_a2c;
}
struct FloatRMat __mul__(struct FloatQuat *q) {
struct FloatRMat m_b2c;
float_rmat_of_quat(&m_b2c, q);
struct FloatRMat m_a2c;
float_rmat_comp(&m_a2c, $self, &m_b2c);
return m_a2c;
}
struct FloatVect3 __mul__(struct FloatVect3 *va) {
struct FloatVect3 v;
float_rmat_vmult(&v, $self, va);
return v;
}
void set_identity() {
float_rmat_identity($self);
}
struct FloatRMat inverse() {
struct FloatRMat inv;
float_rmat_inv(&inv, $self);
return inv;
}
struct FloatRMat transposed() {
struct FloatRMat inv;
float_rmat_inv(&inv, $self);
return inv;
}
/*
void invert() {
float_rmat_inv($self, $self);
}
void transpose() {
float_rmat_inv($self, $self);
}
*/
float norm() {
return float_rmat_norm($self);
}
struct FloatEulers to_eulers() {
struct FloatEulers e;
float_eulers_of_rmat(&e, $self);
return e;
}
struct FloatQuat to_quat() {
struct FloatQuat q;
float_quat_of_rmat(&q, $self);
return q;
}
};
%extend FloatEulers {
char *__str__() {
static char tmp[1024];
sprintf(tmp,"Eulers in deg (phi=%g, theta=%g, psi=%g)", DegOfRad($self->phi),
DegOfRad($self->theta), DegOfRad($self->psi));
return tmp;
}
FloatEulers(float phi=0.0, float theta=0.0, float psi=0.0) {
struct FloatEulers *v = (struct FloatEulers *) malloc(sizeof(struct FloatEulers));
v->phi = phi;
v->theta = theta;
v->psi = psi;
return v;
}
struct FloatQuat to_quat() {
struct FloatQuat q;
float_quat_of_eulers(&q, $self);
return q;
}
struct FloatRMat to_rmat() {
struct FloatRMat r;
float_rmat_of_eulers(&r, $self);
return r;
}
};