mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
mathlib: Vector class major cleanup
This commit is contained in:
Anton Babushkin
parent
a26e46bede
commit
9dfe366e90
@ -94,11 +94,11 @@ public:
|
||||
return data[row][col];
|
||||
}
|
||||
|
||||
unsigned int getRows() {
|
||||
unsigned int get_rows() {
|
||||
return M;
|
||||
}
|
||||
|
||||
unsigned int getCols() {
|
||||
unsigned int get_cols() {
|
||||
return N;
|
||||
}
|
||||
|
||||
@ -295,11 +295,7 @@ public:
|
||||
return res;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
#include "Quaternion.hpp"
|
||||
|
||||
namespace math {
|
||||
template <>
|
||||
class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> {
|
||||
public:
|
||||
@ -329,25 +325,6 @@ public:
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* create a rotation matrix from given quaternion
|
||||
*/
|
||||
void from_quaternion(const Quaternion &q) {
|
||||
float aSq = q.data[0] * q.data[0];
|
||||
float bSq = q.data[1] * q.data[1];
|
||||
float cSq = q.data[2] * q.data[2];
|
||||
float dSq = q.data[3] * q.data[3];
|
||||
data[0][0] = aSq + bSq - cSq - dSq;
|
||||
data[0][1] = 2.0f * (q.data[1] * q.data[2] - q.data[0] * q.data[3]);
|
||||
data[0][2] = 2.0f * (q.data[0] * q.data[2] + q.data[1] * q.data[3]);
|
||||
data[1][0] = 2.0f * (q.data[1] * q.data[2] + q.data[0] * q.data[3]);
|
||||
data[1][1] = aSq - bSq + cSq - dSq;
|
||||
data[1][2] = 2.0f * (q.data[2] * q.data[3] - q.data[0] * q.data[1]);
|
||||
data[2][0] = 2.0f * (q.data[1] * q.data[3] - q.data[0] * q.data[2]);
|
||||
data[2][1] = 2.0f * (q.data[0] * q.data[1] + q.data[2] * q.data[3]);
|
||||
data[2][2] = aSq - bSq - cSq + dSq;
|
||||
}
|
||||
|
||||
/**
|
||||
* create a rotation matrix from given euler angles
|
||||
* based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
|
||||
|
||||
@ -58,7 +58,7 @@ public:
|
||||
/**
|
||||
* trivial ctor
|
||||
*/
|
||||
Quaternion() {
|
||||
Quaternion() : Vector() {
|
||||
}
|
||||
|
||||
/**
|
||||
@ -70,10 +70,29 @@ public:
|
||||
Quaternion(const Vector<4> &v) : Vector(v) {
|
||||
}
|
||||
|
||||
Quaternion(const float *v) : Vector(v) {
|
||||
Quaternion(const Quaternion &q) : Vector(q) {
|
||||
}
|
||||
|
||||
Quaternion derivative(const Vector<3> &w) {
|
||||
Quaternion(const float v[4]) : Vector(v) {
|
||||
}
|
||||
|
||||
using Vector<4>::operator *;
|
||||
|
||||
/**
|
||||
* multiplication
|
||||
*/
|
||||
const Quaternion operator *(const Quaternion &q) const {
|
||||
return Quaternion(
|
||||
data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3],
|
||||
data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2],
|
||||
data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1],
|
||||
data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]);
|
||||
}
|
||||
|
||||
/**
|
||||
* derivative
|
||||
*/
|
||||
const Quaternion derivative(const Vector<3> &w) {
|
||||
float dataQ[] = {
|
||||
data[0], -data[1], -data[2], -data[3],
|
||||
data[1], data[0], -data[3], data[2],
|
||||
@ -85,6 +104,9 @@ public:
|
||||
return Q * v * 0.5f;
|
||||
}
|
||||
|
||||
/**
|
||||
* set quaternion to rotation defined by euler angles
|
||||
*/
|
||||
void from_euler(float roll, float pitch, float yaw) {
|
||||
double cosPhi_2 = cos(double(roll) / 2.0);
|
||||
double sinPhi_2 = sin(double(roll) / 2.0);
|
||||
@ -97,6 +119,26 @@ public:
|
||||
data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2;
|
||||
data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2;
|
||||
}
|
||||
|
||||
/**
|
||||
* create rotation matrix for the quaternion
|
||||
*/
|
||||
Matrix<3, 3> to_dcm(void) const {
|
||||
Matrix<3, 3> R;
|
||||
float aSq = data[0] * data[0];
|
||||
float bSq = data[1] * data[1];
|
||||
float cSq = data[2] * data[2];
|
||||
float dSq = data[3] * data[3];
|
||||
R.data[0][0] = aSq + bSq - cSq - dSq;
|
||||
R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]);
|
||||
R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]);
|
||||
R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]);
|
||||
R.data[1][1] = aSq - bSq + cSq - dSq;
|
||||
R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]);
|
||||
R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]);
|
||||
R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]);
|
||||
R.data[2][2] = aSq - bSq - cSq + dSq;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
@ -1,8 +1,9 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Will Perone <will.perone@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
||||
* Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -36,13 +37,12 @@
|
||||
/**
|
||||
* @file Vector.hpp
|
||||
*
|
||||
* Generic Vector
|
||||
* Vector class
|
||||
*/
|
||||
|
||||
#ifndef VECTOR_HPP
|
||||
#define VECTOR_HPP
|
||||
|
||||
#include <visibility.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include "../CMSIS/Include/arm_math.h"
|
||||
@ -84,7 +84,7 @@ public:
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
VectorBase(const float *d) {
|
||||
VectorBase(const float d[N]) {
|
||||
arm_col = {N, 1, &data[0]};
|
||||
memcpy(data, d, sizeof(data));
|
||||
}
|
||||
@ -92,31 +92,30 @@ public:
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
inline float &operator ()(unsigned int i) {
|
||||
float &operator ()(const unsigned int i) {
|
||||
return data[i];
|
||||
}
|
||||
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
inline const float &operator ()(unsigned int i) const {
|
||||
float operator ()(const unsigned int i) const {
|
||||
return data[i];
|
||||
}
|
||||
|
||||
unsigned int getRows() {
|
||||
/**
|
||||
* get rows number
|
||||
*/
|
||||
unsigned int get_size() const {
|
||||
return N;
|
||||
}
|
||||
|
||||
unsigned int getCols() {
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* test for equality
|
||||
*/
|
||||
bool operator ==(const Vector<N> &v) {
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
if (data[i] != v(i))
|
||||
if (data[i] != v.data[i])
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
@ -126,7 +125,7 @@ public:
|
||||
*/
|
||||
bool operator !=(const Vector<N> &v) {
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
if (data[i] != v(i))
|
||||
if (data[i] != v.data[i])
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
@ -155,7 +154,7 @@ public:
|
||||
const Vector<N> operator +(const Vector<N> &v) const {
|
||||
Vector<N> res;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
res.data[i] = data[i] + v(i);
|
||||
res.data[i] = data[i] + v.data[i];
|
||||
return res;
|
||||
}
|
||||
|
||||
@ -165,7 +164,7 @@ public:
|
||||
const Vector<N> operator -(const Vector<N> &v) const {
|
||||
Vector<N> res;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
res.data[i] = data[i] - v(i);
|
||||
res.data[i] = data[i] - v.data[i];
|
||||
return res;
|
||||
}
|
||||
|
||||
@ -173,16 +172,20 @@ public:
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector<N> operator *(const float num) const {
|
||||
Vector<N> temp(*this);
|
||||
return temp *= num;
|
||||
Vector<N> res;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
res.data[i] = data[i] * num;
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector<N> operator /(const float num) const {
|
||||
Vector<N> temp(*static_cast<const Vector<N>*>(this));
|
||||
return temp /= num;
|
||||
Vector<N> res;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
res.data[i] = data[i] / num;
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -190,7 +193,7 @@ public:
|
||||
*/
|
||||
const Vector<N> &operator +=(const Vector<N> &v) {
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
data[i] += v(i);
|
||||
data[i] += v.data[i];
|
||||
return *static_cast<const Vector<N>*>(this);
|
||||
}
|
||||
|
||||
@ -199,7 +202,7 @@ public:
|
||||
*/
|
||||
const Vector<N> &operator -=(const Vector<N> &v) {
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
data[i] -= v(i);
|
||||
data[i] -= v.data[i];
|
||||
return *static_cast<const Vector<N>*>(this);
|
||||
}
|
||||
|
||||
@ -227,7 +230,7 @@ public:
|
||||
float operator *(const Vector<N> &v) const {
|
||||
float res = 0.0f;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
res += data[i] * v(i);
|
||||
res += data[i] * v.data[i];
|
||||
return res;
|
||||
}
|
||||
|
||||
@ -235,14 +238,20 @@ public:
|
||||
* gets the length of this vector squared
|
||||
*/
|
||||
float length_squared() const {
|
||||
return (*this * *this);
|
||||
float res = 0.0f;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
res += data[i] * data[i];
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* gets the length of this vector
|
||||
*/
|
||||
float length() const {
|
||||
return sqrtf(*this * *static_cast<const Vector<N>*>(this));
|
||||
float res = 0.0f;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
res += data[i] * data[i];
|
||||
return sqrtf(res);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -277,25 +286,17 @@ public:
|
||||
template <unsigned int N>
|
||||
class __EXPORT Vector : public VectorBase<N> {
|
||||
public:
|
||||
using VectorBase<N>::operator *;
|
||||
//using VectorBase<N>::operator *;
|
||||
Vector() : VectorBase<N>() {}
|
||||
|
||||
Vector() : VectorBase<N>() {
|
||||
}
|
||||
Vector(const Vector &v) : VectorBase<N>(v) {}
|
||||
|
||||
Vector(const float d[]) : VectorBase<N>(d) {
|
||||
}
|
||||
|
||||
Vector(const Vector<N> &v) : VectorBase<N>(v) {
|
||||
}
|
||||
|
||||
Vector(const VectorBase<N> &v) : VectorBase<N>(v) {
|
||||
}
|
||||
Vector(const float d[N]) : VectorBase<N>(d) {}
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const Vector<N> &operator =(const Vector<N> &v) {
|
||||
this->arm_col = {N, 1, &this->data[0]};
|
||||
memcpy(this->data, v.data, sizeof(this->data));
|
||||
return *this;
|
||||
}
|
||||
@ -304,24 +305,24 @@ public:
|
||||
template <>
|
||||
class __EXPORT Vector<2> : public VectorBase<2> {
|
||||
public:
|
||||
Vector() : VectorBase<2>() {
|
||||
Vector() : VectorBase<2>() {}
|
||||
|
||||
/* simple copy is 1.6 times faster than memcpy */
|
||||
Vector(const Vector &v) : VectorBase<2>() {
|
||||
data[0] = v.data[0];
|
||||
data[1] = v.data[1];
|
||||
}
|
||||
|
||||
Vector(const float x, const float y) : VectorBase() {
|
||||
Vector(const float d[2]) : VectorBase<2>() {
|
||||
data[0] = d[0];
|
||||
data[1] = d[1];
|
||||
}
|
||||
|
||||
Vector(const float x, const float y) : VectorBase<2>() {
|
||||
data[0] = x;
|
||||
data[1] = y;
|
||||
}
|
||||
|
||||
Vector(const Vector<2> &v) : VectorBase() {
|
||||
data[0] = v.data[0];
|
||||
data[1] = v.data[1];
|
||||
}
|
||||
|
||||
Vector(const VectorBase<2> &v) : VectorBase() {
|
||||
data[0] = v.data[0];
|
||||
data[1] = v.data[1];
|
||||
}
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
@ -331,55 +332,49 @@ public:
|
||||
return *this;
|
||||
}
|
||||
|
||||
float cross(const Vector<2> &b) const {
|
||||
return data[0]*b.data[1] - data[1]*b.data[0];
|
||||
}
|
||||
|
||||
float operator %(const Vector<2> &v) const {
|
||||
return cross(v);
|
||||
return data[0] * v.data[1] - data[1] * v.data[0];
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
template <>
|
||||
class __EXPORT Vector<3> : public VectorBase<3> {
|
||||
public:
|
||||
Vector() {
|
||||
arm_col = {3, 1, &this->data[0]};
|
||||
Vector() : VectorBase<3>() {}
|
||||
|
||||
/* simple copy is 1.6 times faster than memcpy */
|
||||
Vector(const Vector &v) : VectorBase<3>() {
|
||||
for (unsigned int i = 0; i < 3; i++)
|
||||
data[i] = v.data[i];
|
||||
}
|
||||
|
||||
Vector(const float x, const float y, const float z) {
|
||||
arm_col = {3, 1, &this->data[0]};
|
||||
Vector(const float d[3]) : VectorBase<3>() {
|
||||
for (unsigned int i = 0; i < 3; i++)
|
||||
data[i] = d[i];
|
||||
}
|
||||
|
||||
Vector(const float x, const float y, const float z) : VectorBase<3>() {
|
||||
data[0] = x;
|
||||
data[1] = y;
|
||||
data[2] = z;
|
||||
}
|
||||
|
||||
Vector(const Vector<3> &v) : VectorBase<3>() {
|
||||
data[0] = v.data[0];
|
||||
data[1] = v.data[1];
|
||||
data[2] = v.data[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
Vector(const float d[]) {
|
||||
arm_col = {3, 1, &this->data[0]};
|
||||
data[0] = d[0];
|
||||
data[1] = d[1];
|
||||
data[2] = d[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const Vector<3> &operator =(const Vector<3> &v) {
|
||||
data[0] = v.data[0];
|
||||
data[1] = v.data[1];
|
||||
data[2] = v.data[2];
|
||||
for (unsigned int i = 0; i < 3; i++)
|
||||
data[i] = v.data[i];
|
||||
return *this;
|
||||
}
|
||||
|
||||
Vector<3> operator %(const Vector<3> &v) const {
|
||||
return Vector<3>(
|
||||
data[1] * v.data[2] - data[2] * v.data[1],
|
||||
data[2] * v.data[0] - data[0] * v.data[2],
|
||||
data[0] * v.data[1] - data[1] * v.data[0]
|
||||
);
|
||||
}
|
||||
};
|
||||
|
||||
template <>
|
||||
@ -387,49 +382,31 @@ class __EXPORT Vector<4> : public VectorBase<4> {
|
||||
public:
|
||||
Vector() : VectorBase() {}
|
||||
|
||||
Vector(const float x, const float y, const float z, const float t) : VectorBase() {
|
||||
data[0] = x;
|
||||
data[1] = y;
|
||||
data[2] = z;
|
||||
data[3] = t;
|
||||
Vector(const Vector &v) : VectorBase<4>() {
|
||||
for (unsigned int i = 0; i < 4; i++)
|
||||
data[i] = v.data[i];
|
||||
}
|
||||
|
||||
Vector(const Vector<4> &v) : VectorBase() {
|
||||
data[0] = v.data[0];
|
||||
data[1] = v.data[1];
|
||||
data[2] = v.data[2];
|
||||
data[3] = v.data[3];
|
||||
Vector(const float d[4]) : VectorBase<4>() {
|
||||
for (unsigned int i = 0; i < 4; i++)
|
||||
data[i] = d[i];
|
||||
}
|
||||
|
||||
Vector(const VectorBase<4> &v) : VectorBase() {
|
||||
data[0] = v.data[0];
|
||||
data[1] = v.data[1];
|
||||
data[2] = v.data[2];
|
||||
data[3] = v.data[3];
|
||||
Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase() {
|
||||
data[0] = x0;
|
||||
data[1] = x1;
|
||||
data[2] = x2;
|
||||
data[3] = x3;
|
||||
}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
/*
|
||||
Vector(const float d[]) {
|
||||
arm_col = {3, 1, &this->data[0]};
|
||||
data[0] = d[0];
|
||||
data[1] = d[1];
|
||||
data[2] = d[2];
|
||||
}
|
||||
*/
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
/*
|
||||
const Vector<3> &operator =(const Vector<3> &v) {
|
||||
data[0] = v.data[0];
|
||||
data[1] = v.data[1];
|
||||
data[2] = v.data[2];
|
||||
const Vector<4> &operator =(const Vector<4> &v) {
|
||||
for (unsigned int i = 0; i < 4; i++)
|
||||
data[i] = v.data[i];
|
||||
return *this;
|
||||
}
|
||||
*/
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -132,7 +132,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
||||
_sensors.magnetometer_ga[2]);
|
||||
|
||||
// initialize dcm
|
||||
C_nb.from_quaternion(q);
|
||||
C_nb = q.to_dcm();
|
||||
|
||||
// HPos is constant
|
||||
HPos(0, 3) = 1.0f;
|
||||
@ -409,7 +409,7 @@ int KalmanNav::predictState(float dt)
|
||||
}
|
||||
|
||||
// C_nb update
|
||||
C_nb.from_quaternion(q);
|
||||
C_nb = q.to_dcm();
|
||||
|
||||
// euler update
|
||||
Vector<3> euler = C_nb.to_euler();
|
||||
@ -628,7 +628,7 @@ int KalmanNav::correctAtt()
|
||||
Vector<9> xCorrect = K * y;
|
||||
|
||||
// check correciton is sane
|
||||
for (size_t i = 0; i < xCorrect.getRows(); i++) {
|
||||
for (size_t i = 0; i < xCorrect.get_size(); i++) {
|
||||
float val = xCorrect(i);
|
||||
|
||||
if (isnan(val) || isinf(val)) {
|
||||
@ -694,7 +694,7 @@ int KalmanNav::correctPos()
|
||||
Vector<9> xCorrect = K * y;
|
||||
|
||||
// check correction is sane
|
||||
for (size_t i = 0; i < xCorrect.getRows(); i++) {
|
||||
for (size_t i = 0; i < xCorrect.get_size(); i++) {
|
||||
float val = xCorrect(i);
|
||||
|
||||
if (!isfinite(val)) {
|
||||
|
||||
@ -683,8 +683,7 @@ handle_message(mavlink_message_t *msg)
|
||||
|
||||
/* Calculate Rotation Matrix */
|
||||
math::Quaternion q(hil_state.attitude_quaternion);
|
||||
math::Matrix<3,3> C_nb;
|
||||
C_nb.from_quaternion(q);
|
||||
math::Matrix<3,3> C_nb = q.to_dcm();
|
||||
math::Vector<3> euler = C_nb.to_euler();
|
||||
|
||||
/* set rotation matrix */
|
||||
|
||||
@ -465,8 +465,6 @@ Sensors::Sensors() :
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
|
||||
|
||||
_board_rotation(),
|
||||
_external_mag_rotation(),
|
||||
_mag_is_external(false)
|
||||
{
|
||||
|
||||
@ -920,7 +918,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
|
||||
|
||||
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
|
||||
|
||||
math::Vector<3> vect = {accel_report.x, accel_report.y, accel_report.z};
|
||||
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
|
||||
vect = _board_rotation * vect;
|
||||
|
||||
raw.accelerometer_m_s2[0] = vect(0);
|
||||
@ -946,7 +944,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
|
||||
|
||||
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
|
||||
|
||||
math::Vector<3> vect = {gyro_report.x, gyro_report.y, gyro_report.z};
|
||||
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
|
||||
vect = _board_rotation * vect;
|
||||
|
||||
raw.gyro_rad_s[0] = vect(0);
|
||||
|
||||
@ -48,6 +48,8 @@
|
||||
|
||||
#include "tests.h"
|
||||
|
||||
#define TEST_OP(_title, _op) { unsigned int n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); }
|
||||
|
||||
using namespace math;
|
||||
|
||||
const char* formatResult(bool res) {
|
||||
@ -58,60 +60,82 @@ int test_mathlib(int argc, char *argv[])
|
||||
{
|
||||
warnx("testing mathlib");
|
||||
|
||||
Matrix<3,3> m3;
|
||||
m3.identity();
|
||||
Matrix<4,4> m4;
|
||||
m4.identity();
|
||||
Vector<3> v3;
|
||||
v3(0) = 1.0f;
|
||||
v3(1) = 2.0f;
|
||||
v3(2) = 3.0f;
|
||||
Vector<4> v4;
|
||||
v4(0) = 1.0f;
|
||||
v4(1) = 2.0f;
|
||||
v4(2) = 3.0f;
|
||||
v4(3) = 4.0f;
|
||||
Vector<3> vres3;
|
||||
Matrix<3,3> mres3;
|
||||
Matrix<4,4> mres4;
|
||||
Vector<2> v2(1.0f, 2.0f);
|
||||
Vector<3> v3(1.0f, 2.0f, 3.0f);
|
||||
Vector<4> v4(1.0f, 2.0f, 3.0f, 4.0f);
|
||||
Vector<10> v10;
|
||||
v10.zero();
|
||||
|
||||
unsigned int n = 60000;
|
||||
float data2[2] = {1.0f, 2.0f};
|
||||
float data3[3] = {1.0f, 2.0f, 3.0f};
|
||||
float data4[4] = {1.0f, 2.0f, 3.0f, 4.0f};
|
||||
float data10[10];
|
||||
|
||||
hrt_abstime t0, t1;
|
||||
|
||||
t0 = hrt_absolute_time();
|
||||
for (unsigned int j = 0; j < n; j++) {
|
||||
vres3 = m3 * v3;
|
||||
{
|
||||
Vector<2> v;
|
||||
Vector<2> v1(1.0f, 2.0f);
|
||||
Vector<2> v2(1.0f, -1.0f);
|
||||
TEST_OP("Constructor Vector<2>()", Vector<2> v);
|
||||
TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v(v2));
|
||||
TEST_OP("Constructor Vector<2>(float[])", Vector<2> v(data2));
|
||||
TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v(1.0f, 2.0f));
|
||||
TEST_OP("Vector<2> = Vector<2>", v = v1);
|
||||
TEST_OP("Vector<2> + Vector<2>", v + v1);
|
||||
TEST_OP("Vector<2> - Vector<2>", v - v1);
|
||||
TEST_OP("Vector<2> += Vector<2>", v += v1);
|
||||
TEST_OP("Vector<2> -= Vector<2>", v -= v1);
|
||||
TEST_OP("Vector<2> * Vector<2>", v * v1);
|
||||
TEST_OP("Vector<2> %% Vector<2>", v1 % v2);
|
||||
}
|
||||
t1 = hrt_absolute_time();
|
||||
warnx("Matrix3 * Vector3: %s %.6fus", formatResult(vres3 == v3), (double)(t1 - t0) / n);
|
||||
|
||||
t0 = hrt_absolute_time();
|
||||
for (unsigned int j = 0; j < n; j++) {
|
||||
mres3 = m3 * m3;
|
||||
{
|
||||
Vector<3> v;
|
||||
Vector<3> v1(1.0f, 2.0f, 0.0f);
|
||||
Vector<3> v2(1.0f, -1.0f, 2.0f);
|
||||
TEST_OP("Constructor Vector<3>()", Vector<3> v);
|
||||
TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v(v3));
|
||||
TEST_OP("Constructor Vector<3>(float[])", Vector<3> v(data3));
|
||||
TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v(1.0f, 2.0f, 3.0f));
|
||||
TEST_OP("Vector<3> = Vector<3>", v = v1);
|
||||
TEST_OP("Vector<3> + Vector<3>", v + v1);
|
||||
TEST_OP("Vector<3> - Vector<3>", v - v1);
|
||||
TEST_OP("Vector<3> += Vector<3>", v += v1);
|
||||
TEST_OP("Vector<3> -= Vector<3>", v -= v1);
|
||||
TEST_OP("Vector<3> * float", v1 * 2.0f);
|
||||
TEST_OP("Vector<3> / float", v1 / 2.0f);
|
||||
TEST_OP("Vector<3> *= float", v1 *= 2.0f);
|
||||
TEST_OP("Vector<3> /= float", v1 /= 2.0f);
|
||||
TEST_OP("Vector<3> * Vector<3>", v * v1);
|
||||
TEST_OP("Vector<3> %% Vector<3>", v1 % v2);
|
||||
TEST_OP("Vector<3> length", v1.length());
|
||||
TEST_OP("Vector<3> length squared", v1.length_squared());
|
||||
TEST_OP("Vector<3> element read", volatile float a = v1(0));
|
||||
TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]);
|
||||
TEST_OP("Vector<3> element write", v1(0) = 1.0f);
|
||||
TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f);
|
||||
}
|
||||
t1 = hrt_absolute_time();
|
||||
warnx("Matrix3 * Matrix3: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n);
|
||||
|
||||
t0 = hrt_absolute_time();
|
||||
for (unsigned int j = 0; j < n; j++) {
|
||||
mres4 = m4 * m4;
|
||||
{
|
||||
Vector<4> v;
|
||||
Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f);
|
||||
Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f);
|
||||
TEST_OP("Constructor Vector<4>()", Vector<4> v);
|
||||
TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v(v4));
|
||||
TEST_OP("Constructor Vector<4>(float[])", Vector<4> v(data4));
|
||||
TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v(1.0f, 2.0f, 3.0f, 4.0f));
|
||||
TEST_OP("Vector<4> = Vector<4>", v = v1);
|
||||
TEST_OP("Vector<4> + Vector<4>", v + v1);
|
||||
TEST_OP("Vector<4> - Vector<4>", v - v1);
|
||||
TEST_OP("Vector<4> += Vector<4>", v += v1);
|
||||
TEST_OP("Vector<4> -= Vector<4>", v -= v1);
|
||||
TEST_OP("Vector<4> * Vector<4>", v * v1);
|
||||
}
|
||||
t1 = hrt_absolute_time();
|
||||
warnx("Matrix4 * Matrix4: %s %.6fus", formatResult(mres4 == m4), (double)(t1 - t0) / n);
|
||||
|
||||
t0 = hrt_absolute_time();
|
||||
for (unsigned int j = 0; j < n; j++) {
|
||||
mres3 = m3.transposed();
|
||||
{
|
||||
TEST_OP("Constructor Vector<10>()", Vector<10> v);
|
||||
TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v(v10));
|
||||
TEST_OP("Constructor Vector<10>(float[])", Vector<10> v(data10));
|
||||
}
|
||||
t1 = hrt_absolute_time();
|
||||
warnx("Matrix3 Transpose: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n);
|
||||
|
||||
t0 = hrt_absolute_time();
|
||||
for (unsigned int j = 0; j < n; j++) {
|
||||
mres3 = m3.inversed();
|
||||
}
|
||||
t1 = hrt_absolute_time();
|
||||
warnx("Matrix3 Invert: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n);
|
||||
return 0;
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user