forked from Archive/PX4-Autopilot
New mathlib, WIP
This commit is contained in:
parent
ea107f4151
commit
a83e3cd222
|
@ -0,0 +1,50 @@
|
|||
#
|
||||
# Makefile for the px4fmu_default configuration
|
||||
#
|
||||
|
||||
#
|
||||
# Use the configuration's ROMFS.
|
||||
#
|
||||
ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test
|
||||
|
||||
#
|
||||
# Board support modules
|
||||
#
|
||||
MODULES += drivers/device
|
||||
MODULES += drivers/stm32
|
||||
MODULES += drivers/led
|
||||
MODULES += drivers/boards/px4fmu-v1
|
||||
MODULES += systemcmds/perf
|
||||
MODULES += systemcmds/reboot
|
||||
MODULES += systemcmds/tests
|
||||
MODULES += systemcmds/nshterm
|
||||
|
||||
#
|
||||
# Libraries
|
||||
#
|
||||
LIBRARIES += lib/mathlib/CMSIS
|
||||
MODULES += lib/mathlib
|
||||
|
||||
#
|
||||
# Library modules
|
||||
#
|
||||
MODULES += modules/systemlib
|
||||
MODULES += modules/uORB
|
||||
MODULES += modules/systemlib/mixer
|
||||
|
||||
#
|
||||
# Transitional support - add commands from the NuttX export archive.
|
||||
#
|
||||
# In general, these should move to modules over time.
|
||||
#
|
||||
# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
|
||||
# to make the table a bit more readable.
|
||||
#
|
||||
define _B
|
||||
$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
|
||||
endef
|
||||
|
||||
# command priority stack entrypoint
|
||||
BUILTIN_COMMANDS := \
|
||||
$(call _B, sercon, , 2048, sercon_main ) \
|
||||
$(call _B, serdis, , 2048, serdis_main )
|
|
@ -41,22 +41,11 @@
|
|||
#include "rotation.h"
|
||||
|
||||
__EXPORT void
|
||||
get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
|
||||
get_rot_matrix(enum Rotation rot, math::Matrix3f *rot_matrix)
|
||||
{
|
||||
/* first set to zero */
|
||||
rot_matrix->Matrix::zero(3, 3);
|
||||
|
||||
float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
|
||||
float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
|
||||
float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
|
||||
|
||||
math::EulerAngles euler(roll, pitch, yaw);
|
||||
|
||||
math::Dcm R(euler);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
(*rot_matrix)(i, j) = R(i, j);
|
||||
}
|
||||
}
|
||||
rot_matrix->from_euler(roll, pitch, yaw);
|
||||
}
|
||||
|
|
|
@ -1,174 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Dcm.cpp
|
||||
*
|
||||
* math direction cosine matrix
|
||||
*/
|
||||
|
||||
#include <mathlib/math/test/test.hpp>
|
||||
|
||||
#include "Dcm.hpp"
|
||||
#include "Quaternion.hpp"
|
||||
#include "EulerAngles.hpp"
|
||||
#include "Vector3.hpp"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
Dcm::Dcm() :
|
||||
Matrix(Matrix::identity(3))
|
||||
{
|
||||
}
|
||||
|
||||
Dcm::Dcm(float c00, float c01, float c02,
|
||||
float c10, float c11, float c12,
|
||||
float c20, float c21, float c22) :
|
||||
Matrix(3, 3)
|
||||
{
|
||||
Dcm &dcm = *this;
|
||||
dcm(0, 0) = c00;
|
||||
dcm(0, 1) = c01;
|
||||
dcm(0, 2) = c02;
|
||||
dcm(1, 0) = c10;
|
||||
dcm(1, 1) = c11;
|
||||
dcm(1, 2) = c12;
|
||||
dcm(2, 0) = c20;
|
||||
dcm(2, 1) = c21;
|
||||
dcm(2, 2) = c22;
|
||||
}
|
||||
|
||||
Dcm::Dcm(const float data[3][3]) :
|
||||
Matrix(3, 3)
|
||||
{
|
||||
Dcm &dcm = *this;
|
||||
/* set rotation matrix */
|
||||
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
|
||||
dcm(i, j) = data[i][j];
|
||||
}
|
||||
|
||||
Dcm::Dcm(const float *data) :
|
||||
Matrix(3, 3, data)
|
||||
{
|
||||
}
|
||||
|
||||
Dcm::Dcm(const Quaternion &q) :
|
||||
Matrix(3, 3)
|
||||
{
|
||||
Dcm &dcm = *this;
|
||||
double a = q.getA();
|
||||
double b = q.getB();
|
||||
double c = q.getC();
|
||||
double d = q.getD();
|
||||
double aSq = a * a;
|
||||
double bSq = b * b;
|
||||
double cSq = c * c;
|
||||
double dSq = d * d;
|
||||
dcm(0, 0) = aSq + bSq - cSq - dSq;
|
||||
dcm(0, 1) = 2.0 * (b * c - a * d);
|
||||
dcm(0, 2) = 2.0 * (a * c + b * d);
|
||||
dcm(1, 0) = 2.0 * (b * c + a * d);
|
||||
dcm(1, 1) = aSq - bSq + cSq - dSq;
|
||||
dcm(1, 2) = 2.0 * (c * d - a * b);
|
||||
dcm(2, 0) = 2.0 * (b * d - a * c);
|
||||
dcm(2, 1) = 2.0 * (a * b + c * d);
|
||||
dcm(2, 2) = aSq - bSq - cSq + dSq;
|
||||
}
|
||||
|
||||
Dcm::Dcm(const EulerAngles &euler) :
|
||||
Matrix(3, 3)
|
||||
{
|
||||
Dcm &dcm = *this;
|
||||
double cosPhi = cos(euler.getPhi());
|
||||
double sinPhi = sin(euler.getPhi());
|
||||
double cosThe = cos(euler.getTheta());
|
||||
double sinThe = sin(euler.getTheta());
|
||||
double cosPsi = cos(euler.getPsi());
|
||||
double sinPsi = sin(euler.getPsi());
|
||||
|
||||
dcm(0, 0) = cosThe * cosPsi;
|
||||
dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
|
||||
dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
|
||||
|
||||
dcm(1, 0) = cosThe * sinPsi;
|
||||
dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
|
||||
dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
|
||||
|
||||
dcm(2, 0) = -sinThe;
|
||||
dcm(2, 1) = sinPhi * cosThe;
|
||||
dcm(2, 2) = cosPhi * cosThe;
|
||||
}
|
||||
|
||||
Dcm::Dcm(const Dcm &right) :
|
||||
Matrix(right)
|
||||
{
|
||||
}
|
||||
|
||||
Dcm::~Dcm()
|
||||
{
|
||||
}
|
||||
|
||||
int __EXPORT dcmTest()
|
||||
{
|
||||
printf("Test DCM\t\t: ");
|
||||
// default ctor
|
||||
ASSERT(matrixEqual(Dcm(),
|
||||
Matrix::identity(3)));
|
||||
// quaternion ctor
|
||||
ASSERT(matrixEqual(
|
||||
Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)),
|
||||
Dcm(0.9362934f, -0.2750958f, 0.2183507f,
|
||||
0.2896295f, 0.9564251f, -0.0369570f,
|
||||
-0.1986693f, 0.0978434f, 0.9751703f)));
|
||||
// euler angle ctor
|
||||
ASSERT(matrixEqual(
|
||||
Dcm(EulerAngles(0.1f, 0.2f, 0.3f)),
|
||||
Dcm(0.9362934f, -0.2750958f, 0.2183507f,
|
||||
0.2896295f, 0.9564251f, -0.0369570f,
|
||||
-0.1986693f, 0.0978434f, 0.9751703f)));
|
||||
// rotations
|
||||
Vector3 vB(1, 2, 3);
|
||||
ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f),
|
||||
Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB));
|
||||
ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
|
||||
Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB));
|
||||
ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f),
|
||||
Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB));
|
||||
ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
|
||||
Dcm(EulerAngles(
|
||||
M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
} // namespace math
|
|
@ -1,108 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Dcm.hpp
|
||||
*
|
||||
* math direction cosine matrix
|
||||
*/
|
||||
|
||||
//#pragma once
|
||||
|
||||
#include "Vector.hpp"
|
||||
#include "Matrix.hpp"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
class Quaternion;
|
||||
class EulerAngles;
|
||||
|
||||
/**
|
||||
* This is a Tait Bryan, Body 3-2-1 sequence.
|
||||
* (yaw)-(pitch)-(roll)
|
||||
* The Dcm transforms a vector in the body frame
|
||||
* to the navigation frame, typically represented
|
||||
* as C_nb. C_bn can be obtained through use
|
||||
* of the transpose() method.
|
||||
*/
|
||||
class __EXPORT Dcm : public Matrix
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* default ctor
|
||||
*/
|
||||
Dcm();
|
||||
|
||||
/**
|
||||
* scalar ctor
|
||||
*/
|
||||
Dcm(float c00, float c01, float c02,
|
||||
float c10, float c11, float c12,
|
||||
float c20, float c21, float c22);
|
||||
|
||||
/**
|
||||
* data ctor
|
||||
*/
|
||||
Dcm(const float *data);
|
||||
|
||||
/**
|
||||
* array ctor
|
||||
*/
|
||||
Dcm(const float data[3][3]);
|
||||
|
||||
/**
|
||||
* quaternion ctor
|
||||
*/
|
||||
Dcm(const Quaternion &q);
|
||||
|
||||
/**
|
||||
* euler angles ctor
|
||||
*/
|
||||
Dcm(const EulerAngles &euler);
|
||||
|
||||
/**
|
||||
* copy ctor (deep)
|
||||
*/
|
||||
Dcm(const Dcm &right);
|
||||
|
||||
/**
|
||||
* dtor
|
||||
*/
|
||||
virtual ~Dcm();
|
||||
};
|
||||
|
||||
int __EXPORT dcmTest();
|
||||
|
||||
} // math
|
||||
|
|
@ -1,126 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Vector.cpp
|
||||
*
|
||||
* math vector
|
||||
*/
|
||||
|
||||
#include "test/test.hpp"
|
||||
|
||||
#include "EulerAngles.hpp"
|
||||
#include "Quaternion.hpp"
|
||||
#include "Dcm.hpp"
|
||||
#include "Vector3.hpp"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
EulerAngles::EulerAngles() :
|
||||
Vector(3)
|
||||
{
|
||||
setPhi(0.0f);
|
||||
setTheta(0.0f);
|
||||
setPsi(0.0f);
|
||||
}
|
||||
|
||||
EulerAngles::EulerAngles(float phi, float theta, float psi) :
|
||||
Vector(3)
|
||||
{
|
||||
setPhi(phi);
|
||||
setTheta(theta);
|
||||
setPsi(psi);
|
||||
}
|
||||
|
||||
EulerAngles::EulerAngles(const Quaternion &q) :
|
||||
Vector(3)
|
||||
{
|
||||
(*this) = EulerAngles(Dcm(q));
|
||||
}
|
||||
|
||||
EulerAngles::EulerAngles(const Dcm &dcm) :
|
||||
Vector(3)
|
||||
{
|
||||
setTheta(asinf(-dcm(2, 0)));
|
||||
|
||||
if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) {
|
||||
setPhi(0.0f);
|
||||
setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
|
||||
dcm(0, 2) + dcm(1, 1)) + getPhi());
|
||||
|
||||
} else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) {
|
||||
setPhi(0.0f);
|
||||
setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
|
||||
dcm(0, 2) + dcm(1, 1)) - getPhi());
|
||||
|
||||
} else {
|
||||
setPhi(atan2f(dcm(2, 1), dcm(2, 2)));
|
||||
setPsi(atan2f(dcm(1, 0), dcm(0, 0)));
|
||||
}
|
||||
}
|
||||
|
||||
EulerAngles::~EulerAngles()
|
||||
{
|
||||
}
|
||||
|
||||
int __EXPORT eulerAnglesTest()
|
||||
{
|
||||
printf("Test EulerAngles\t: ");
|
||||
EulerAngles euler(0.1f, 0.2f, 0.3f);
|
||||
|
||||
// test ctor
|
||||
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
|
||||
ASSERT(equal(euler.getPhi(), 0.1f));
|
||||
ASSERT(equal(euler.getTheta(), 0.2f));
|
||||
ASSERT(equal(euler.getPsi(), 0.3f));
|
||||
|
||||
// test dcm ctor
|
||||
euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f));
|
||||
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
|
||||
|
||||
// test quat ctor
|
||||
euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
|
||||
ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
|
||||
|
||||
// test assignment
|
||||
euler.setPhi(0.4f);
|
||||
euler.setTheta(0.5f);
|
||||
euler.setPsi(0.6f);
|
||||
ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler));
|
||||
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // namespace math
|
|
@ -1,74 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Vector.h
|
||||
*
|
||||
* math vector
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Vector.hpp"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
class Quaternion;
|
||||
class Dcm;
|
||||
|
||||
class __EXPORT EulerAngles : public Vector
|
||||
{
|
||||
public:
|
||||
EulerAngles();
|
||||
EulerAngles(float phi, float theta, float psi);
|
||||
EulerAngles(const Quaternion &q);
|
||||
EulerAngles(const Dcm &dcm);
|
||||
virtual ~EulerAngles();
|
||||
|
||||
// alias
|
||||
void setPhi(float phi) { (*this)(0) = phi; }
|
||||
void setTheta(float theta) { (*this)(1) = theta; }
|
||||
void setPsi(float psi) { (*this)(2) = psi; }
|
||||
|
||||
// const accessors
|
||||
const float &getPhi() const { return (*this)(0); }
|
||||
const float &getTheta() const { return (*this)(1); }
|
||||
const float &getPsi() const { return (*this)(2); }
|
||||
|
||||
};
|
||||
|
||||
int __EXPORT eulerAnglesTest();
|
||||
|
||||
} // math
|
||||
|
|
@ -1,193 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Matrix.cpp
|
||||
*
|
||||
* matrix code
|
||||
*/
|
||||
|
||||
#include "test/test.hpp"
|
||||
#include <math.h>
|
||||
|
||||
#include "Matrix.hpp"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
static const float data_testA[] = {
|
||||
1, 2, 3,
|
||||
4, 5, 6
|
||||
};
|
||||
static Matrix testA(2, 3, data_testA);
|
||||
|
||||
static const float data_testB[] = {
|
||||
0, 1, 3,
|
||||
7, -1, 2
|
||||
};
|
||||
static Matrix testB(2, 3, data_testB);
|
||||
|
||||
static const float data_testC[] = {
|
||||
0, 1,
|
||||
2, 1,
|
||||
3, 2
|
||||
};
|
||||
static Matrix testC(3, 2, data_testC);
|
||||
|
||||
static const float data_testD[] = {
|
||||
0, 1, 2,
|
||||
2, 1, 4,
|
||||
5, 2, 0
|
||||
};
|
||||
static Matrix testD(3, 3, data_testD);
|
||||
|
||||
static const float data_testE[] = {
|
||||
1, -1, 2,
|
||||
0, 2, 3,
|
||||
2, -1, 1
|
||||
};
|
||||
static Matrix testE(3, 3, data_testE);
|
||||
|
||||
static const float data_testF[] = {
|
||||
3.777e006f, 2.915e007f, 0.000e000f,
|
||||
2.938e007f, 2.267e008f, 0.000e000f,
|
||||
0.000e000f, 0.000e000f, 6.033e008f
|
||||
};
|
||||
static Matrix testF(3, 3, data_testF);
|
||||
|
||||
int __EXPORT matrixTest()
|
||||
{
|
||||
matrixAddTest();
|
||||
matrixSubTest();
|
||||
matrixMultTest();
|
||||
matrixInvTest();
|
||||
matrixDivTest();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int matrixAddTest()
|
||||
{
|
||||
printf("Test Matrix Add\t\t: ");
|
||||
Matrix r = testA + testB;
|
||||
float data_test[] = {
|
||||
1.0f, 3.0f, 6.0f,
|
||||
11.0f, 4.0f, 8.0f
|
||||
};
|
||||
ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int matrixSubTest()
|
||||
{
|
||||
printf("Test Matrix Sub\t\t: ");
|
||||
Matrix r = testA - testB;
|
||||
float data_test[] = {
|
||||
1.0f, 1.0f, 0.0f,
|
||||
-3.0f, 6.0f, 4.0f
|
||||
};
|
||||
ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int matrixMultTest()
|
||||
{
|
||||
printf("Test Matrix Mult\t: ");
|
||||
Matrix r = testC * testB;
|
||||
float data_test[] = {
|
||||
7.0f, -1.0f, 2.0f,
|
||||
7.0f, 1.0f, 8.0f,
|
||||
14.0f, 1.0f, 13.0f
|
||||
};
|
||||
ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int matrixInvTest()
|
||||
{
|
||||
printf("Test Matrix Inv\t\t: ");
|
||||
Matrix origF = testF;
|
||||
Matrix r = testF.inverse();
|
||||
float data_test[] = {
|
||||
-0.0012518f, 0.0001610f, 0.0000000f,
|
||||
0.0001622f, -0.0000209f, 0.0000000f,
|
||||
0.0000000f, 0.0000000f, 1.6580e-9f
|
||||
};
|
||||
ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
|
||||
// make sure F in unchanged
|
||||
ASSERT(matrixEqual(origF, testF));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int matrixDivTest()
|
||||
{
|
||||
printf("Test Matrix Div\t\t: ");
|
||||
Matrix r = testD / testE;
|
||||
float data_test[] = {
|
||||
0.2222222f, 0.5555556f, -0.1111111f,
|
||||
0.0f, 1.0f, 1.0,
|
||||
-4.1111111f, 1.2222222f, 4.5555556f
|
||||
};
|
||||
ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool matrixEqual(const Matrix &a, const Matrix &b, float eps)
|
||||
{
|
||||
if (a.getRows() != b.getRows()) {
|
||||
printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
|
||||
return false;
|
||||
|
||||
} else if (a.getCols() != b.getCols()) {
|
||||
printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols());
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ret = true;
|
||||
|
||||
for (size_t i = 0; i < a.getRows(); i++)
|
||||
for (size_t j = 0; j < a.getCols(); j++) {
|
||||
if (!equal(a(i, j), b(i, j), eps)) {
|
||||
printf("element mismatch (%d, %d)\n", i, j);
|
||||
ret = false;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
} // namespace math
|
|
@ -1,6 +1,8 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Will Perone <will.perone@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
@ -32,30 +34,241 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Matrix.h
|
||||
* @file Matrix3.hpp
|
||||
*
|
||||
* matrix code
|
||||
* 3x3 Matrix
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#ifndef MATRIX_HPP
|
||||
#define MATRIX_HPP
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
|
||||
#include "arm/Matrix.hpp"
|
||||
#else
|
||||
#include "generic/Matrix.hpp"
|
||||
#endif
|
||||
#include "../CMSIS/Include/arm_math.h"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
template <unsigned int N, unsigned int M>
|
||||
class Matrix;
|
||||
int matrixTest();
|
||||
int matrixAddTest();
|
||||
int matrixSubTest();
|
||||
int matrixMultTest();
|
||||
int matrixInvTest();
|
||||
int matrixDivTest();
|
||||
int matrixArmTest();
|
||||
bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f);
|
||||
} // namespace math
|
||||
|
||||
// MxN matrix with float elements
|
||||
template <unsigned int N, unsigned int M>
|
||||
class MatrixBase {
|
||||
public:
|
||||
/**
|
||||
* matrix data[row][col]
|
||||
*/
|
||||
float data[M][N];
|
||||
|
||||
/**
|
||||
* struct for using arm_math functions
|
||||
*/
|
||||
arm_matrix_instance_f32 arm_mat;
|
||||
|
||||
/**
|
||||
* trivial ctor
|
||||
* note that this ctor will not initialize elements
|
||||
*/
|
||||
MatrixBase() {
|
||||
//arm_mat_init_f32(&arm_mat, M, N, data);
|
||||
arm_mat = {M, N, &data[0][0]};
|
||||
}
|
||||
|
||||
/**
|
||||
* access by index
|
||||
*/
|
||||
inline float &operator ()(unsigned int row, unsigned int col) {
|
||||
return data[row][col];
|
||||
}
|
||||
|
||||
/**
|
||||
* access by index
|
||||
*/
|
||||
inline const float &operator ()(unsigned int row, unsigned int col) const {
|
||||
return data[row][col];
|
||||
}
|
||||
|
||||
unsigned int getRows() {
|
||||
return M;
|
||||
}
|
||||
|
||||
unsigned int getCols() {
|
||||
return N;
|
||||
}
|
||||
|
||||
/**
|
||||
* test for equality
|
||||
*/
|
||||
bool operator ==(const MatrixBase<M, N> &m) {
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
if (data[i][j] != m(i, j))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* test for inequality
|
||||
*/
|
||||
bool operator !=(const MatrixBase<M, N> &m) {
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
if (data[i][j] != m(i, j))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const MatrixBase<M, N> &operator =(const MatrixBase<M, N> &m) {
|
||||
memcpy(data, m.data, sizeof(data));
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* negation
|
||||
*/
|
||||
MatrixBase<M, N> operator -(void) const {
|
||||
MatrixBase<M, N> res;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
for (unsigned int j = 0; j < M; j++)
|
||||
res[i][j] = -data[i][j];
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
MatrixBase<M, N> operator +(const MatrixBase<M, N> &m) const {
|
||||
MatrixBase<M, N> res;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
for (unsigned int j = 0; j < M; j++)
|
||||
res[i][j] = data[i][j] + m(i, j);
|
||||
return res;
|
||||
}
|
||||
|
||||
MatrixBase<M, N> &operator +=(const MatrixBase<M, N> &m) {
|
||||
return *this = *this + m;
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
MatrixBase<M, N> operator -(const MatrixBase<M, N> &m) const {
|
||||
MatrixBase<M, N> res;
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
res[i][j] = data[i][j] - m(i, j);
|
||||
return res;
|
||||
}
|
||||
|
||||
MatrixBase<M, N> &operator -=(const MatrixBase<M, N> &m) {
|
||||
return *this = *this - m;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
MatrixBase<M, N> operator *(const float num) const {
|
||||
MatrixBase<M, N> res;
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
res[i][j] = data[i][j] * num;
|
||||
return res;
|
||||
}
|
||||
|
||||
MatrixBase<M, N> &operator *=(const float num) {
|
||||
return *this = *this * num;
|
||||
}
|
||||
|
||||
MatrixBase<M, N> operator /(const float num) const {
|
||||
MatrixBase<M, N> res;
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
res[i][j] = data[i][j] / num;
|
||||
return res;
|
||||
}
|
||||
|
||||
MatrixBase<M, N> &operator /=(const float num) {
|
||||
return *this = *this / num;
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication by another matrix
|
||||
*/
|
||||
template <unsigned int P>
|
||||
Matrix<N, P> operator *(const Matrix<M, P> &m) const {
|
||||
Matrix<N, P> res;
|
||||
arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* setup the identity matrix
|
||||
*/
|
||||
void identity(void) {
|
||||
memset(data, 0, sizeof(data));
|
||||
for (unsigned int i = 0; i < M; i++)
|
||||
data[i][i] = 1;
|
||||
}
|
||||
|
||||
void dump(void) {
|
||||
for (unsigned int i = 0; i < M; i++) {
|
||||
for (unsigned int j = 0; j < N; j++)
|
||||
printf("%.3f\t", data[i][j]);
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
template <unsigned int M, unsigned int N>
|
||||
class Matrix : public MatrixBase<M, N> {
|
||||
public:
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
|
||||
memcpy(this->data, m.data, sizeof(this->data));
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication by a vector
|
||||
*/
|
||||
/*
|
||||
Vector<N> operator *(const Vector<N> &v) const {
|
||||
Vector<M> res;
|
||||
arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
|
||||
return res;
|
||||
}
|
||||
*/
|
||||
};
|
||||
|
||||
template <>
|
||||
class Matrix<3, 3> : public MatrixBase<3, 3> {
|
||||
public:
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const Matrix<3, 3> &operator =(const Matrix<3, 3> &m) {
|
||||
memcpy(this->data, m.data, sizeof(this->data));
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication by a vector
|
||||
*/
|
||||
/*
|
||||
Vector<3> operator *(const Vector<3> &v) const {
|
||||
Vector<3> res;
|
||||
res(0) = data[0][0] * v(0) + data[0][1] * v(1) + data[0][2] * v(2);
|
||||
res(1) = data[1][0] * v(0) + data[1][1] * v(1) + data[1][2] * v(2);
|
||||
res(2) = data[2][0] * v(0) + data[2][1] * v(1) + data[2][2] * v(2);
|
||||
return res;
|
||||
}
|
||||
*/
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // MATRIX_HPP
|
||||
|
|
|
@ -1,6 +1,8 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Will Perone <will.perone@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
@ -32,9 +34,13 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Matrix.cpp
|
||||
* @file Matrix3.cpp
|
||||
*
|
||||
* matrix code
|
||||
* 3x3 Matrix
|
||||
*/
|
||||
|
||||
#include "Matrix.hpp"
|
||||
#include "Matrix3.hpp"
|
||||
|
||||
namespace math
|
||||
{
|
||||
}
|
|
@ -0,0 +1,348 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Will Perone <will.perone@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Matrix3.hpp
|
||||
*
|
||||
* 3x3 Matrix
|
||||
*/
|
||||
|
||||
#ifndef MATRIX3_HPP
|
||||
#define MATRIX3_HPP
|
||||
|
||||
#include "Vector3.hpp"
|
||||
#include "../CMSIS/Include/arm_math.h"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
// 3x3 matrix with elements of type T
|
||||
template <typename T>
|
||||
class Matrix3 {
|
||||
public:
|
||||
/**
|
||||
* matrix data[row][col]
|
||||
*/
|
||||
T data[3][3];
|
||||
|
||||
/**
|
||||
* struct for using arm_math functions
|
||||
*/
|
||||
arm_matrix_instance_f32 arm_mat;
|
||||
|
||||
/**
|
||||
* trivial ctor
|
||||
* note that this ctor will not initialize elements
|
||||
*/
|
||||
Matrix3<T>() {
|
||||
arm_mat = {3, 3, &data[0][0]};
|
||||
}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
Matrix3<T>(const T d[3][3]) {
|
||||
memcpy(data, d, sizeof(data));
|
||||
arm_mat = {3, 3, &data[0][0]};
|
||||
}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
Matrix3<T>(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz) {
|
||||
data[0][0] = ax;
|
||||
data[0][1] = ay;
|
||||
data[0][2] = az;
|
||||
data[1][0] = bx;
|
||||
data[1][1] = by;
|
||||
data[1][2] = bz;
|
||||
data[2][0] = cx;
|
||||
data[2][1] = cy;
|
||||
data[2][2] = cz;
|
||||
arm_mat = {3, 3, &data[0][0]};
|
||||
}
|
||||
|
||||
/**
|
||||
* casting from a vector3f to a matrix is the tilde operator
|
||||
*/
|
||||
Matrix3<T>(const Vector3<T> &v) {
|
||||
arm_mat = {3, 3, &data[0][0]};
|
||||
data[0][0] = 0;
|
||||
data[0][1] = -v.z;
|
||||
data[0][2] = v.y;
|
||||
data[1][0] = v.z;
|
||||
data[1][1] = 0;
|
||||
data[1][2] = -v.x;
|
||||
data[2][0] = -v.y;
|
||||
data[2][1] = v.x;
|
||||
data[2][2] = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* access by index
|
||||
*/
|
||||
inline T &operator ()(unsigned int row, unsigned int col) {
|
||||
return data[row][col];
|
||||
}
|
||||
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
inline const T &operator ()(unsigned int row, unsigned int col) const {
|
||||
return data[row][col];
|
||||
}
|
||||
|
||||
/**
|
||||
* test for equality
|
||||
*/
|
||||
bool operator ==(const Matrix3<T> &m) {
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
if (data[i][j] != m(i, j))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* test for inequality
|
||||
*/
|
||||
bool operator !=(const Matrix3<T> &m) {
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
if (data[i][j] != m(i, j))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* negation
|
||||
*/
|
||||
Matrix3<T> operator -(void) const {
|
||||
Matrix3<T> res;
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
res[i][j] = -data[i][j];
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
Matrix3<T> operator +(const Matrix3<T> &m) const {
|
||||
Matrix3<T> res;
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
res[i][j] = data[i][j] + m(i, j);
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix3<T> &operator +=(const Matrix3<T> &m) {
|
||||
return *this = *this + m;
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
Matrix3<T> operator -(const Matrix3<T> &m) const {
|
||||
Matrix3<T> res;
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
res[i][j] = data[i][j] - m(i, j);
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix3<T> &operator -=(const Matrix3<T> &m) {
|
||||
return *this = *this - m;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
Matrix3<T> operator *(const T num) const {
|
||||
Matrix3<T> res;
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
res[i][j] = data[i][j] * num;
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix3<T> &operator *=(const T num) {
|
||||
return *this = *this * num;
|
||||
}
|
||||
|
||||
Matrix3<T> operator /(const T num) const {
|
||||
Matrix3<T> res;
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
res[i][j] = data[i][j] / num;
|
||||
return res;
|
||||
}
|
||||
|
||||
Matrix3<T> &operator /=(const T num) {
|
||||
return *this = *this / num;
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication by a vector
|
||||
*/
|
||||
Vector3<T> operator *(const Vector3<T> &v) const {
|
||||
return Vector3<T>(
|
||||
data[0][0] * v.x + data[0][1] * v.y + data[0][2] * v.z,
|
||||
data[1][0] * v.x + data[1][1] * v.y + data[1][2] * v.z,
|
||||
data[2][0] * v.x + data[2][1] * v.y + data[2][2] * v.z);
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication of transpose by a vector
|
||||
*/
|
||||
Vector3<T> mul_transpose(const Vector3<T> &v) const {
|
||||
return Vector3<T>(
|
||||
data[0][0] * v.x + data[1][0] * v.y + data[2][0] * v.z,
|
||||
data[0][1] * v.x + data[1][1] * v.y + data[2][1] * v.z,
|
||||
data[0][2] * v.x + data[1][2] * v.y + data[2][2] * v.z);
|
||||
}
|
||||
|
||||
/**
|
||||
* multiplication by another matrix
|
||||
*/
|
||||
Matrix3<T> operator *(const Matrix3<T> &m) const {
|
||||
#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
|
||||
Matrix3<T> res;
|
||||
arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
|
||||
return res;
|
||||
#else
|
||||
return Matrix3<T>(data[0][0] * m(0, 0) + data[0][1] * m(1, 0) + data[0][2] * m(2, 0),
|
||||
data[0][0] * m(0, 1) + data[0][1] * m(1, 1) + data[0][2] * m(2, 1),
|
||||
data[0][0] * m(0, 2) + data[0][1] * m(1, 2) + data[0][2] * m(2, 2),
|
||||
data[1][0] * m(0, 0) + data[1][1] * m(1, 0) + data[1][2] * m(2, 0),
|
||||
data[1][0] * m(0, 1) + data[1][1] * m(1, 1) + data[1][2] * m(2, 1),
|
||||
data[1][0] * m(0, 2) + data[1][1] * m(1, 2) + data[1][2] * m(2, 2),
|
||||
data[2][0] * m(0, 0) + data[2][1] * m(1, 0) + data[2][2] * m(2, 0),
|
||||
data[2][0] * m(0, 1) + data[2][1] * m(1, 1) + data[2][2] * m(2, 1),
|
||||
data[2][0] * m(0, 2) + data[2][1] * m(1, 2) + data[2][2] * m(2, 2));
|
||||
#endif
|
||||
}
|
||||
|
||||
Matrix3<T> &operator *=(const Matrix3<T> &m) {
|
||||
return *this = *this * m;
|
||||
}
|
||||
|
||||
/**
|
||||
* transpose the matrix
|
||||
*/
|
||||
Matrix3<T> transposed(void) const {
|
||||
#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) && T == float
|
||||
Matrix3<T> res;
|
||||
arm_mat_trans_f32(&arm_mat, &res.arm_mat);
|
||||
return res;
|
||||
#else
|
||||
return Matrix3<T>(data[0][0], data[1][0], data[2][0],
|
||||
data[0][1], data[1][1], data[2][1],
|
||||
data[0][2], data[1][2], data[2][2]);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* inverse the matrix
|
||||
*/
|
||||
Matrix3<T> inversed(void) const {
|
||||
Matrix3<T> res;
|
||||
arm_mat_inverse_f32(&arm_mat, &res.arm_mat);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* zero the matrix
|
||||
*/
|
||||
void zero(void) {
|
||||
memset(data, 0, sizeof(data));
|
||||
}
|
||||
|
||||
/**
|
||||
* setup the identity matrix
|
||||
*/
|
||||
void identity(void) {
|
||||
memset(data, 0, sizeof(data));
|
||||
data[0][0] = 1;
|
||||
data[1][1] = 1;
|
||||
data[2][2] = 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* check if any elements are NAN
|
||||
*/
|
||||
bool is_nan(void) {
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
if (isnan(data[i][j]))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* create a rotation matrix from given euler angles
|
||||
* based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
|
||||
*/
|
||||
void from_euler(T roll, T pitch, T yaw) {
|
||||
T cp = (T)cosf(pitch);
|
||||
T sp = (T)sinf(pitch);
|
||||
T sr = (T)sinf(roll);
|
||||
T cr = (T)cosf(roll);
|
||||
T sy = (T)sinf(yaw);
|
||||
T cy = (T)cosf(yaw);
|
||||
|
||||
data[0][0] = cp * cy;
|
||||
data[0][1] = (sr * sp * cy) - (cr * sy);
|
||||
data[0][2] = (cr * sp * cy) + (sr * sy);
|
||||
data[1][0] = cp * sy;
|
||||
data[1][1] = (sr * sp * sy) + (cr * cy);
|
||||
data[1][2] = (cr * sp * sy) - (sr * cy);
|
||||
data[2][0] = -sp;
|
||||
data[2][1] = sr * cp;
|
||||
data[2][2] = cr * cp;
|
||||
}
|
||||
|
||||
// create eulers from a rotation matrix
|
||||
//void to_euler(float *roll, float *pitch, float *yaw);
|
||||
|
||||
// apply an additional rotation from a body frame gyro vector
|
||||
// to a rotation matrix.
|
||||
//void rotate(const Vector3<T> &g);
|
||||
};
|
||||
|
||||
typedef Matrix3<float> Matrix3f;
|
||||
}
|
||||
|
||||
#endif // MATRIX3_HPP
|
|
@ -1,174 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Quaternion.cpp
|
||||
*
|
||||
* math vector
|
||||
*/
|
||||
|
||||
#include "test/test.hpp"
|
||||
|
||||
|
||||
#include "Quaternion.hpp"
|
||||
#include "Dcm.hpp"
|
||||
#include "EulerAngles.hpp"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
Quaternion::Quaternion() :
|
||||
Vector(4)
|
||||
{
|
||||
setA(1.0f);
|
||||
setB(0.0f);
|
||||
setC(0.0f);
|
||||
setD(0.0f);
|
||||
}
|
||||
|
||||
Quaternion::Quaternion(float a, float b,
|
||||
float c, float d) :
|
||||
Vector(4)
|
||||
{
|
||||
setA(a);
|
||||
setB(b);
|
||||
setC(c);
|
||||
setD(d);
|
||||
}
|
||||
|
||||
Quaternion::Quaternion(const float *data) :
|
||||
Vector(4, data)
|
||||
{
|
||||
}
|
||||
|
||||
Quaternion::Quaternion(const Vector &v) :
|
||||
Vector(v)
|
||||
{
|
||||
}
|
||||
|
||||
Quaternion::Quaternion(const Dcm &dcm) :
|
||||
Vector(4)
|
||||
{
|
||||
// avoiding singularities by not using
|
||||
// division equations
|
||||
setA(0.5 * sqrt(1.0 +
|
||||
double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2))));
|
||||
setB(0.5 * sqrt(1.0 +
|
||||
double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2))));
|
||||
setC(0.5 * sqrt(1.0 +
|
||||
double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2))));
|
||||
setD(0.5 * sqrt(1.0 +
|
||||
double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2))));
|
||||
}
|
||||
|
||||
Quaternion::Quaternion(const EulerAngles &euler) :
|
||||
Vector(4)
|
||||
{
|
||||
double cosPhi_2 = cos(double(euler.getPhi()) / 2.0);
|
||||
double sinPhi_2 = sin(double(euler.getPhi()) / 2.0);
|
||||
double cosTheta_2 = cos(double(euler.getTheta()) / 2.0);
|
||||
double sinTheta_2 = sin(double(euler.getTheta()) / 2.0);
|
||||
double cosPsi_2 = cos(double(euler.getPsi()) / 2.0);
|
||||
double sinPsi_2 = sin(double(euler.getPsi()) / 2.0);
|
||||
setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
|
||||
sinPhi_2 * sinTheta_2 * sinPsi_2);
|
||||
setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
|
||||
cosPhi_2 * sinTheta_2 * sinPsi_2);
|
||||
setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
|
||||
sinPhi_2 * cosTheta_2 * sinPsi_2);
|
||||
setD(cosPhi_2 * cosTheta_2 * sinPsi_2 -
|
||||
sinPhi_2 * sinTheta_2 * cosPsi_2);
|
||||
}
|
||||
|
||||
Quaternion::Quaternion(const Quaternion &right) :
|
||||
Vector(right)
|
||||
{
|
||||
}
|
||||
|
||||
Quaternion::~Quaternion()
|
||||
{
|
||||
}
|
||||
|
||||
Vector Quaternion::derivative(const Vector &w)
|
||||
{
|
||||
#ifdef QUATERNION_ASSERT
|
||||
ASSERT(w.getRows() == 3);
|
||||
#endif
|
||||
float dataQ[] = {
|
||||
getA(), -getB(), -getC(), -getD(),
|
||||
getB(), getA(), -getD(), getC(),
|
||||
getC(), getD(), getA(), -getB(),
|
||||
getD(), -getC(), getB(), getA()
|
||||
};
|
||||
Vector v(4);
|
||||
v(0) = 0.0f;
|
||||
v(1) = w(0);
|
||||
v(2) = w(1);
|
||||
v(3) = w(2);
|
||||
Matrix Q(4, 4, dataQ);
|
||||
return Q * v * 0.5f;
|
||||
}
|
||||
|
||||
int __EXPORT quaternionTest()
|
||||
{
|
||||
printf("Test Quaternion\t\t: ");
|
||||
// test default ctor
|
||||
Quaternion q;
|
||||
ASSERT(equal(q.getA(), 1.0f));
|
||||
ASSERT(equal(q.getB(), 0.0f));
|
||||
ASSERT(equal(q.getC(), 0.0f));
|
||||
ASSERT(equal(q.getD(), 0.0f));
|
||||
// test float ctor
|
||||
q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
|
||||
ASSERT(equal(q.getA(), 0.1825742f));
|
||||
ASSERT(equal(q.getB(), 0.3651484f));
|
||||
ASSERT(equal(q.getC(), 0.5477226f));
|
||||
ASSERT(equal(q.getD(), 0.7302967f));
|
||||
// test euler ctor
|
||||
q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
|
||||
ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)));
|
||||
// test dcm ctor
|
||||
q = Quaternion(Dcm());
|
||||
ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f)));
|
||||
// TODO test derivative
|
||||
// test accessors
|
||||
q.setA(0.1f);
|
||||
q.setB(0.2f);
|
||||
q.setC(0.3f);
|
||||
q.setD(0.4f);
|
||||
ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f)));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // namespace math
|
|
@ -1,6 +1,8 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Will Perone <will.perone@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
@ -34,82 +36,35 @@
|
|||
/**
|
||||
* @file Quaternion.hpp
|
||||
*
|
||||
* math quaternion lib
|
||||
* Quaternion
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#ifndef QUATERNION_HPP
|
||||
#define QUATERNION_HPP
|
||||
|
||||
#include "Vector.hpp"
|
||||
#include "Matrix.hpp"
|
||||
#include <math.h>
|
||||
#include "../CMSIS/Include/arm_math.h"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
class Dcm;
|
||||
class EulerAngles;
|
||||
|
||||
class __EXPORT Quaternion : public Vector
|
||||
{
|
||||
class Quaternion {
|
||||
public:
|
||||
float real;
|
||||
Vector3<float> imag;
|
||||
|
||||
/**
|
||||
* default ctor
|
||||
* trivial ctor
|
||||
*/
|
||||
Quaternion();
|
||||
Quaternion() {
|
||||
}
|
||||
|
||||
/**
|
||||
* ctor from floats
|
||||
* setting ctor
|
||||
*/
|
||||
Quaternion(float a, float b, float c, float d);
|
||||
|
||||
/**
|
||||
* ctor from data
|
||||
*/
|
||||
Quaternion(const float *data);
|
||||
|
||||
/**
|
||||
* ctor from Vector
|
||||
*/
|
||||
Quaternion(const Vector &v);
|
||||
|
||||
/**
|
||||
* ctor from EulerAngles
|
||||
*/
|
||||
Quaternion(const EulerAngles &euler);
|
||||
|
||||
/**
|
||||
* ctor from Dcm
|
||||
*/
|
||||
Quaternion(const Dcm &dcm);
|
||||
|
||||
/**
|
||||
* deep copy ctor
|
||||
*/
|
||||
Quaternion(const Quaternion &right);
|
||||
|
||||
/**
|
||||
* dtor
|
||||
*/
|
||||
virtual ~Quaternion();
|
||||
|
||||
/**
|
||||
* derivative
|
||||
*/
|
||||
Vector derivative(const Vector &w);
|
||||
|
||||
/**
|
||||
* accessors
|
||||
*/
|
||||
void setA(float a) { (*this)(0) = a; }
|
||||
void setB(float b) { (*this)(1) = b; }
|
||||
void setC(float c) { (*this)(2) = c; }
|
||||
void setD(float d) { (*this)(3) = d; }
|
||||
const float &getA() const { return (*this)(0); }
|
||||
const float &getB() const { return (*this)(1); }
|
||||
const float &getC() const { return (*this)(2); }
|
||||
const float &getD() const { return (*this)(3); }
|
||||
Quaternion(const float a, const float b, const float c, const float d): real(a), imag(b, c, d) {
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
int __EXPORT quaternionTest();
|
||||
} // math
|
||||
|
||||
#endif // QUATERNION_HPP
|
||||
|
|
|
@ -1,100 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Vector.cpp
|
||||
*
|
||||
* math vector
|
||||
*/
|
||||
|
||||
#include "test/test.hpp"
|
||||
|
||||
#include "Vector.hpp"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
static const float data_testA[] = {1, 3};
|
||||
static const float data_testB[] = {4, 1};
|
||||
|
||||
static Vector testA(2, data_testA);
|
||||
static Vector testB(2, data_testB);
|
||||
|
||||
int __EXPORT vectorTest()
|
||||
{
|
||||
vectorAddTest();
|
||||
vectorSubTest();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int vectorAddTest()
|
||||
{
|
||||
printf("Test Vector Add\t\t: ");
|
||||
Vector r = testA + testB;
|
||||
float data_test[] = {5.0f, 4.0f};
|
||||
ASSERT(vectorEqual(Vector(2, data_test), r));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int vectorSubTest()
|
||||
{
|
||||
printf("Test Vector Sub\t\t: ");
|
||||
Vector r(2);
|
||||
r = testA - testB;
|
||||
float data_test[] = { -3.0f, 2.0f};
|
||||
ASSERT(vectorEqual(Vector(2, data_test), r));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool vectorEqual(const Vector &a, const Vector &b, float eps)
|
||||
{
|
||||
if (a.getRows() != b.getRows()) {
|
||||
printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ret = true;
|
||||
|
||||
for (size_t i = 0; i < a.getRows(); i++) {
|
||||
if (!equal(a(i), b(i), eps)) {
|
||||
printf("element mismatch (%d)\n", i);
|
||||
ret = false;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
} // namespace math
|
|
@ -1,6 +1,8 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Will Perone <will.perone@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
@ -32,26 +34,204 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Vector.h
|
||||
* @file Vector.hpp
|
||||
*
|
||||
* math vector
|
||||
* Generic Vector
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#ifndef VECTOR_HPP
|
||||
#define VECTOR_HPP
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
|
||||
#include "arm/Vector.hpp"
|
||||
#else
|
||||
#include "generic/Vector.hpp"
|
||||
#endif
|
||||
#include <math.h>
|
||||
#include "../CMSIS/Include/arm_math.h"
|
||||
|
||||
namespace math
|
||||
{
|
||||
class Vector;
|
||||
int __EXPORT vectorTest();
|
||||
int __EXPORT vectorAddTest();
|
||||
int __EXPORT vectorSubTest();
|
||||
bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f);
|
||||
} // math
|
||||
|
||||
template <unsigned int N>
|
||||
class Vector {
|
||||
public:
|
||||
float data[N];
|
||||
arm_matrix_instance_f32 arm_col;
|
||||
|
||||
/**
|
||||
* trivial ctor
|
||||
*/
|
||||
Vector<N>() {
|
||||
arm_col = {N, 1, &data[0]};
|
||||
}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
Vector<N>(const float *d) {
|
||||
memcpy(data, d, sizeof(data));
|
||||
arm_col = {N, 1, &data[0]};
|
||||
}
|
||||
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
inline float &operator ()(unsigned int i) {
|
||||
return data[i];
|
||||
}
|
||||
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
inline const float &operator ()(unsigned int i) const {
|
||||
return data[i];
|
||||
}
|
||||
|
||||
/**
|
||||
* test for equality
|
||||
*/
|
||||
bool operator ==(const Vector<N> &v) {
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
if (data[i] != v(i))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* test for inequality
|
||||
*/
|
||||
bool operator !=(const Vector<N> &v) {
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
if (data[i] != v(i))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const Vector<N> &operator =(const Vector<N> &v) {
|
||||
memcpy(data, v.data, sizeof(data));
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* negation
|
||||
*/
|
||||
const Vector<N> operator -(void) const {
|
||||
Vector<N> res;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
res[i] = -data[i];
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
const Vector<N> operator +(const Vector<N> &v) const {
|
||||
Vector<N> res;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
res[i] = data[i] + v(i);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
const Vector<N> operator -(const Vector<N> &v) const {
|
||||
Vector<N> res;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
res[i] = data[i] - v(i);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector<N> operator *(const float num) const {
|
||||
Vector<N> temp(*this);
|
||||
return temp *= num;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector<N> operator /(const float num) const {
|
||||
Vector<N> temp(*this);
|
||||
return temp /= num;
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
const Vector<N> &operator +=(const Vector<N> &v) {
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
data[i] += v(i);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
const Vector<N> &operator -=(const Vector<N> &v) {
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
data[i] -= v(i);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector<N> &operator *=(const float num) {
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
data[i] *= num;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector<N> &operator /=(const float num) {
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
data[i] /= num;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* dot product
|
||||
*/
|
||||
float operator *(const Vector<N> &v) const {
|
||||
float res;
|
||||
for (unsigned int i = 0; i < N; i++)
|
||||
res += data[i] * v(i);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* gets the length of this vector squared
|
||||
*/
|
||||
float length_squared() const {
|
||||
return (*this * *this);
|
||||
}
|
||||
|
||||
/**
|
||||
* gets the length of this vector
|
||||
*/
|
||||
float length() const {
|
||||
return sqrtf(*this * *this);
|
||||
}
|
||||
|
||||
/**
|
||||
* normalizes this vector
|
||||
*/
|
||||
void normalize() {
|
||||
*this /= length();
|
||||
}
|
||||
|
||||
/**
|
||||
* returns the normalized version of this vector
|
||||
*/
|
||||
Vector<N> normalized() const {
|
||||
return *this / length();
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // VECTOR_HPP
|
||||
|
|
|
@ -0,0 +1,269 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Will Perone <will.perone@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Vector2.hpp
|
||||
*
|
||||
* 2D Vector
|
||||
*/
|
||||
|
||||
#ifndef VECTOR2_HPP
|
||||
#define VECTOR2_HPP
|
||||
|
||||
#include <math.h>
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
template <typename T>
|
||||
class Vector2 {
|
||||
public:
|
||||
T x, y;
|
||||
|
||||
/**
|
||||
* trivial ctor
|
||||
*/
|
||||
Vector2<T>() {
|
||||
}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
Vector2<T>(const T x0, const T y0): x(x0), y(y0) {
|
||||
}
|
||||
|
||||
/**
|
||||
* setter
|
||||
*/
|
||||
void set(const T x0, const T y0) {
|
||||
x = x0;
|
||||
y = y0;
|
||||
}
|
||||
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
T operator ()(unsigned int i) {
|
||||
return *(&x + i);
|
||||
}
|
||||
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
const T operator ()(unsigned int i) const {
|
||||
return *(&x + i);
|
||||
}
|
||||
|
||||
/**
|
||||
* test for equality
|
||||
*/
|
||||
bool operator ==(const Vector2<T> &v) {
|
||||
return (x == v.x && y == v.y);
|
||||
}
|
||||
|
||||
/**
|
||||
* test for inequality
|
||||
*/
|
||||
bool operator !=(const Vector2<T> &v) {
|
||||
return (x != v.x || y != v.y);
|
||||
}
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const Vector2<T> &operator =(const Vector2<T> &v) {
|
||||
x = v.x;
|
||||
y = v.y;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* negation
|
||||
*/
|
||||
const Vector2<T> operator -(void) const {
|
||||
return Vector2<T>(-x, -y);
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
const Vector2<T> operator +(const Vector2<T> &v) const {
|
||||
return Vector2<T>(x + v.x, y + v.y);
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
const Vector2<T> operator -(const Vector2<T> &v) const {
|
||||
return Vector2<T>(x - v.x, y - v.y);
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector2<T> operator *(const T num) const {
|
||||
Vector2<T> temp(*this);
|
||||
return temp *= num;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector2<T> operator /(const T num) const {
|
||||
Vector2<T> temp(*this);
|
||||
return temp /= num;
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
const Vector2<T> &operator +=(const Vector2<T> &v) {
|
||||
x += v.x;
|
||||
y += v.y;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
const Vector2<T> &operator -=(const Vector2<T> &v) {
|
||||
x -= v.x;
|
||||
y -= v.y;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector2<T> &operator *=(const T num) {
|
||||
x *= num;
|
||||
y *= num;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector2<T> &operator /=(const T num) {
|
||||
x /= num;
|
||||
y /= num;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* dot product
|
||||
*/
|
||||
T operator *(const Vector2<T> &v) const {
|
||||
return x * v.x + y * v.y;
|
||||
}
|
||||
|
||||
/**
|
||||
* cross product
|
||||
*/
|
||||
const float operator %(const Vector2<T> &v) const {
|
||||
return x * v.y - y * v.x;
|
||||
}
|
||||
|
||||
/**
|
||||
* gets the length of this vector squared
|
||||
*/
|
||||
float length_squared() const {
|
||||
return (*this * *this);
|
||||
}
|
||||
|
||||
/**
|
||||
* gets the length of this vector
|
||||
*/
|
||||
float length() const {
|
||||
return (T)sqrt(*this * *this);
|
||||
}
|
||||
|
||||
/**
|
||||
* normalizes this vector
|
||||
*/
|
||||
void normalize() {
|
||||
*this /= length();
|
||||
}
|
||||
|
||||
/**
|
||||
* returns the normalized version of this vector
|
||||
*/
|
||||
Vector2<T> normalized() const {
|
||||
return *this / length();
|
||||
}
|
||||
|
||||
/**
|
||||
* reflects this vector about n
|
||||
*/
|
||||
void reflect(const Vector2<T> &n)
|
||||
{
|
||||
Vector2<T> orig(*this);
|
||||
project(n);
|
||||
*this = *this * 2 - orig;
|
||||
}
|
||||
|
||||
/**
|
||||
* projects this vector onto v
|
||||
*/
|
||||
void project(const Vector2<T> &v) {
|
||||
*this = v * (*this * v) / (v * v);
|
||||
}
|
||||
|
||||
/**
|
||||
* returns this vector projected onto v
|
||||
*/
|
||||
Vector2<T> projected(const Vector2<T> &v) {
|
||||
return v * (*this * v) / (v * v);
|
||||
}
|
||||
|
||||
/**
|
||||
* computes the angle between 2 arbitrary vectors
|
||||
*/
|
||||
static inline float angle(const Vector2<T> &v1, const Vector2<T> &v2) {
|
||||
return acosf((v1 * v2) / (v1.length() * v2.length()));
|
||||
}
|
||||
|
||||
/**
|
||||
* computes the angle between 2 arbitrary normalized vectors
|
||||
*/
|
||||
static inline float angle_normalized(const Vector2<T> &v1, const Vector2<T> &v2) {
|
||||
return acosf(v1 * v2);
|
||||
}
|
||||
};
|
||||
|
||||
typedef Vector2<float> Vector2f;
|
||||
}
|
||||
|
||||
#endif // VECTOR2_HPP
|
|
@ -1,79 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Vector2f.hpp
|
||||
*
|
||||
* math 3 vector
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Vector.hpp"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
class __EXPORT Vector2f :
|
||||
public Vector
|
||||
{
|
||||
public:
|
||||
Vector2f();
|
||||
Vector2f(const Vector &right);
|
||||
Vector2f(float x, float y);
|
||||
Vector2f(const float *data);
|
||||
virtual ~Vector2f();
|
||||
float cross(const Vector2f &b) const;
|
||||
float operator %(const Vector2f &v) const;
|
||||
float operator *(const Vector2f &v) const;
|
||||
inline Vector2f operator*(const float &right) const {
|
||||
return Vector::operator*(right);
|
||||
}
|
||||
|
||||
/**
|
||||
* accessors
|
||||
*/
|
||||
void setX(float x) { (*this)(0) = x; }
|
||||
void setY(float y) { (*this)(1) = y; }
|
||||
const float &getX() const { return (*this)(0); }
|
||||
const float &getY() const { return (*this)(1); }
|
||||
};
|
||||
|
||||
class __EXPORT Vector2 :
|
||||
public Vector2f
|
||||
{
|
||||
};
|
||||
|
||||
int __EXPORT vector2fTest();
|
||||
} // math
|
||||
|
|
@ -1,99 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Vector3.cpp
|
||||
*
|
||||
* math vector
|
||||
*/
|
||||
|
||||
#include "test/test.hpp"
|
||||
|
||||
#include "Vector3.hpp"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
Vector3::Vector3() :
|
||||
Vector(3)
|
||||
{
|
||||
}
|
||||
|
||||
Vector3::Vector3(const Vector &right) :
|
||||
Vector(right)
|
||||
{
|
||||
#ifdef VECTOR_ASSERT
|
||||
ASSERT(right.getRows() == 3);
|
||||
#endif
|
||||
}
|
||||
|
||||
Vector3::Vector3(float x, float y, float z) :
|
||||
Vector(3)
|
||||
{
|
||||
setX(x);
|
||||
setY(y);
|
||||
setZ(z);
|
||||
}
|
||||
|
||||
Vector3::Vector3(const float *data) :
|
||||
Vector(3, data)
|
||||
{
|
||||
}
|
||||
|
||||
Vector3::~Vector3()
|
||||
{
|
||||
}
|
||||
|
||||
Vector3 Vector3::cross(const Vector3 &b) const
|
||||
{
|
||||
const Vector3 &a = *this;
|
||||
Vector3 result;
|
||||
result(0) = a(1) * b(2) - a(2) * b(1);
|
||||
result(1) = a(2) * b(0) - a(0) * b(2);
|
||||
result(2) = a(0) * b(1) - a(1) * b(0);
|
||||
return result;
|
||||
}
|
||||
|
||||
int __EXPORT vector3Test()
|
||||
{
|
||||
printf("Test Vector3\t\t: ");
|
||||
// test float ctor
|
||||
Vector3 v(1, 2, 3);
|
||||
ASSERT(equal(v(0), 1));
|
||||
ASSERT(equal(v(1), 2));
|
||||
ASSERT(equal(v(2), 3));
|
||||
printf("PASS\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // namespace math
|
|
@ -1,6 +1,8 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Will Perone <will.perone@gmail.com>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
@ -34,43 +36,252 @@
|
|||
/**
|
||||
* @file Vector3.hpp
|
||||
*
|
||||
* math 3 vector
|
||||
* 3D Vector
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#ifndef VECTOR3_HPP
|
||||
#define VECTOR3_HPP
|
||||
|
||||
#include "Vector.hpp"
|
||||
#include <math.h>
|
||||
#include "../CMSIS/Include/arm_math.h"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
class __EXPORT Vector3 :
|
||||
public Vector
|
||||
{
|
||||
template <typename T>
|
||||
class Vector3 {
|
||||
public:
|
||||
Vector3();
|
||||
Vector3(const Vector &right);
|
||||
Vector3(float x, float y, float z);
|
||||
Vector3(const float *data);
|
||||
virtual ~Vector3();
|
||||
Vector3 cross(const Vector3 &b) const;
|
||||
T x, y, z;
|
||||
arm_matrix_instance_f32 arm_col;
|
||||
|
||||
/**
|
||||
* accessors
|
||||
* trivial ctor
|
||||
*/
|
||||
void setX(float x) { (*this)(0) = x; }
|
||||
void setY(float y) { (*this)(1) = y; }
|
||||
void setZ(float z) { (*this)(2) = z; }
|
||||
const float &getX() const { return (*this)(0); }
|
||||
const float &getY() const { return (*this)(1); }
|
||||
const float &getZ() const { return (*this)(2); }
|
||||
};
|
||||
|
||||
class __EXPORT Vector3f :
|
||||
public Vector3
|
||||
{
|
||||
Vector3<T>() {
|
||||
arm_col = {3, 1, &x};
|
||||
}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
Vector3<T>(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) {
|
||||
arm_col = {3, 1, &x};
|
||||
}
|
||||
|
||||
/**
|
||||
* setting ctor
|
||||
*/
|
||||
Vector3<T>(const T data[3]): x(data[0]), y(data[1]), z(data[2]) {
|
||||
arm_col = {3, 1, &x};
|
||||
}
|
||||
|
||||
/**
|
||||
* setter
|
||||
*/
|
||||
void set(const T x0, const T y0, const T z0) {
|
||||
x = x0;
|
||||
y = y0;
|
||||
z = z0;
|
||||
}
|
||||
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
T operator ()(unsigned int i) {
|
||||
return *(&x + i);
|
||||
}
|
||||
|
||||
/**
|
||||
* access to elements by index
|
||||
*/
|
||||
const T operator ()(unsigned int i) const {
|
||||
return *(&x + i);
|
||||
}
|
||||
|
||||
/**
|
||||
* test for equality
|
||||
*/
|
||||
bool operator ==(const Vector3<T> &v) {
|
||||
return (x == v.x && y == v.y && z == v.z);
|
||||
}
|
||||
|
||||
/**
|
||||
* test for inequality
|
||||
*/
|
||||
bool operator !=(const Vector3<T> &v) {
|
||||
return (x != v.x || y != v.y || z != v.z);
|
||||
}
|
||||
|
||||
/**
|
||||
* set to value
|
||||
*/
|
||||
const Vector3<T> &operator =(const Vector3<T> &v) {
|
||||
x = v.x;
|
||||
y = v.y;
|
||||
z = v.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* negation
|
||||
*/
|
||||
const Vector3<T> operator -(void) const {
|
||||
return Vector3<T>(-x, -y, -z);
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
const Vector3<T> operator +(const Vector3<T> &v) const {
|
||||
return Vector3<T>(x + v.x, y + v.y, z + v.z);
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
const Vector3<T> operator -(const Vector3<T> &v) const {
|
||||
return Vector3<T>(x - v.x, y - v.y, z - v.z);
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector3<T> operator *(const T num) const {
|
||||
Vector3<T> temp(*this);
|
||||
return temp *= num;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector3<T> operator /(const T num) const {
|
||||
Vector3<T> temp(*this);
|
||||
return temp /= num;
|
||||
}
|
||||
|
||||
/**
|
||||
* addition
|
||||
*/
|
||||
const Vector3<T> &operator +=(const Vector3<T> &v) {
|
||||
x += v.x;
|
||||
y += v.y;
|
||||
z += v.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* subtraction
|
||||
*/
|
||||
const Vector3<T> &operator -=(const Vector3<T> &v) {
|
||||
x -= v.x;
|
||||
y -= v.y;
|
||||
z -= v.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector3<T> &operator *=(const T num) {
|
||||
x *= num;
|
||||
y *= num;
|
||||
z *= num;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* uniform scaling
|
||||
*/
|
||||
const Vector3<T> &operator /=(const T num) {
|
||||
x /= num;
|
||||
y /= num;
|
||||
z /= num;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* dot product
|
||||
*/
|
||||
T operator *(const Vector3<T> &v) const {
|
||||
return x * v.x + y * v.y + z * v.z;
|
||||
}
|
||||
|
||||
/**
|
||||
* cross product
|
||||
*/
|
||||
const Vector3<T> operator %(const Vector3<T> &v) const {
|
||||
Vector3<T> temp(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
|
||||
return temp;
|
||||
}
|
||||
|
||||
/**
|
||||
* gets the length of this vector squared
|
||||
*/
|
||||
float length_squared() const {
|
||||
return (*this * *this);
|
||||
}
|
||||
|
||||
/**
|
||||
* gets the length of this vector
|
||||
*/
|
||||
float length() const {
|
||||
return (T)sqrt(*this * *this);
|
||||
}
|
||||
|
||||
/**
|
||||
* normalizes this vector
|
||||
*/
|
||||
void normalize() {
|
||||
*this /= length();
|
||||
}
|
||||
|
||||
/**
|
||||
* returns the normalized version of this vector
|
||||
*/
|
||||
Vector3<T> normalized() const {
|
||||
return *this / length();
|
||||
}
|
||||
|
||||
/**
|
||||
* reflects this vector about n
|
||||
*/
|
||||
void reflect(const Vector3<T> &n)
|
||||
{
|
||||
Vector3<T> orig(*this);
|
||||
project(n);
|
||||
*this = *this * 2 - orig;
|
||||
}
|
||||
|
||||
/**
|
||||
* projects this vector onto v
|
||||
*/
|
||||
void project(const Vector3<T> &v) {
|
||||
*this = v * (*this * v) / (v * v);
|
||||
}
|
||||
|
||||
/**
|
||||
* returns this vector projected onto v
|
||||
*/
|
||||
Vector3<T> projected(const Vector3<T> &v) {
|
||||
return v * (*this * v) / (v * v);
|
||||
}
|
||||
|
||||
/**
|
||||
* computes the angle between 2 arbitrary vectors
|
||||
*/
|
||||
static inline float angle(const Vector3<T> &v1, const Vector3<T> &v2) {
|
||||
return acosf((v1 * v2) / (v1.length() * v2.length()));
|
||||
}
|
||||
|
||||
/**
|
||||
* computes the angle between 2 arbitrary normalized vectors
|
||||
*/
|
||||
static inline float angle_normalized(const Vector3<T> &v1, const Vector3<T> &v2) {
|
||||
return acosf(v1 * v2);
|
||||
}
|
||||
};
|
||||
|
||||
int __EXPORT vector3Test();
|
||||
} // math
|
||||
typedef Vector3<float> Vector3f;
|
||||
}
|
||||
|
||||
#endif // VECTOR3_HPP
|
||||
|
|
|
@ -1,40 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Matrix.cpp
|
||||
*
|
||||
* matrix code
|
||||
*/
|
||||
|
||||
#include "Matrix.hpp"
|
|
@ -1,292 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Matrix.h
|
||||
*
|
||||
* matrix code
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <assert.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "../Vector.hpp"
|
||||
#include "../Matrix.hpp"
|
||||
|
||||
// arm specific
|
||||
#include "../../CMSIS/Include/arm_math.h"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
class __EXPORT Matrix
|
||||
{
|
||||
public:
|
||||
// constructor
|
||||
Matrix(size_t rows, size_t cols) :
|
||||
_matrix() {
|
||||
arm_mat_init_f32(&_matrix,
|
||||
rows, cols,
|
||||
(float *)calloc(rows * cols, sizeof(float)));
|
||||
}
|
||||
Matrix(size_t rows, size_t cols, const float *data) :
|
||||
_matrix() {
|
||||
arm_mat_init_f32(&_matrix,
|
||||
rows, cols,
|
||||
(float *)malloc(rows * cols * sizeof(float)));
|
||||
memcpy(getData(), data, getSize());
|
||||
}
|
||||
// deconstructor
|
||||
virtual ~Matrix() {
|
||||
delete [] _matrix.pData;
|
||||
}
|
||||
// copy constructor (deep)
|
||||
Matrix(const Matrix &right) :
|
||||
_matrix() {
|
||||
arm_mat_init_f32(&_matrix,
|
||||
right.getRows(), right.getCols(),
|
||||
(float *)malloc(right.getRows()*
|
||||
right.getCols()*sizeof(float)));
|
||||
memcpy(getData(), right.getData(),
|
||||
getSize());
|
||||
}
|
||||
// assignment
|
||||
inline Matrix &operator=(const Matrix &right) {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(getRows() == right.getRows());
|
||||
ASSERT(getCols() == right.getCols());
|
||||
#endif
|
||||
|
||||
if (this != &right) {
|
||||
memcpy(getData(), right.getData(),
|
||||
right.getSize());
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
// element accessors
|
||||
inline float &operator()(size_t i, size_t j) {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(i < getRows());
|
||||
ASSERT(j < getCols());
|
||||
#endif
|
||||
return getData()[i * getCols() + j];
|
||||
}
|
||||
inline const float &operator()(size_t i, size_t j) const {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(i < getRows());
|
||||
ASSERT(j < getCols());
|
||||
#endif
|
||||
return getData()[i * getCols() + j];
|
||||
}
|
||||
// output
|
||||
inline void print() const {
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
float sig;
|
||||
int exponent;
|
||||
float num = (*this)(i, j);
|
||||
float2SigExp(num, sig, exponent);
|
||||
printf("%6.3fe%03d ", (double)sig, exponent);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
// boolean ops
|
||||
inline bool operator==(const Matrix &right) const {
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
// scalar ops
|
||||
inline Matrix operator+(float right) const {
|
||||
Matrix result(getRows(), getCols());
|
||||
arm_offset_f32((float *)getData(), right,
|
||||
(float *)result.getData(), getRows()*getCols());
|
||||
return result;
|
||||
}
|
||||
inline Matrix operator-(float right) const {
|
||||
Matrix result(getRows(), getCols());
|
||||
arm_offset_f32((float *)getData(), -right,
|
||||
(float *)result.getData(), getRows()*getCols());
|
||||
return result;
|
||||
}
|
||||
inline Matrix operator*(float right) const {
|
||||
Matrix result(getRows(), getCols());
|
||||
arm_mat_scale_f32(&_matrix, right,
|
||||
&(result._matrix));
|
||||
return result;
|
||||
}
|
||||
inline Matrix operator/(float right) const {
|
||||
Matrix result(getRows(), getCols());
|
||||
arm_mat_scale_f32(&_matrix, 1.0f / right,
|
||||
&(result._matrix));
|
||||
return result;
|
||||
}
|
||||
// vector ops
|
||||
inline Vector operator*(const Vector &right) const {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(getCols() == right.getRows());
|
||||
#endif
|
||||
Matrix resultMat = (*this) *
|
||||
Matrix(right.getRows(), 1, right.getData());
|
||||
return Vector(getRows(), resultMat.getData());
|
||||
}
|
||||
// matrix ops
|
||||
inline Matrix operator+(const Matrix &right) const {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(getRows() == right.getRows());
|
||||
ASSERT(getCols() == right.getCols());
|
||||
#endif
|
||||
Matrix result(getRows(), getCols());
|
||||
arm_mat_add_f32(&_matrix, &(right._matrix),
|
||||
&(result._matrix));
|
||||
return result;
|
||||
}
|
||||
inline Matrix operator-(const Matrix &right) const {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(getRows() == right.getRows());
|
||||
ASSERT(getCols() == right.getCols());
|
||||
#endif
|
||||
Matrix result(getRows(), getCols());
|
||||
arm_mat_sub_f32(&_matrix, &(right._matrix),
|
||||
&(result._matrix));
|
||||
return result;
|
||||
}
|
||||
inline Matrix operator*(const Matrix &right) const {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(getCols() == right.getRows());
|
||||
#endif
|
||||
Matrix result(getRows(), right.getCols());
|
||||
arm_mat_mult_f32(&_matrix, &(right._matrix),
|
||||
&(result._matrix));
|
||||
return result;
|
||||
}
|
||||
inline Matrix operator/(const Matrix &right) const {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(right.getRows() == right.getCols());
|
||||
ASSERT(getCols() == right.getCols());
|
||||
#endif
|
||||
return (*this) * right.inverse();
|
||||
}
|
||||
// other functions
|
||||
inline Matrix transpose() const {
|
||||
Matrix result(getCols(), getRows());
|
||||
arm_mat_trans_f32(&_matrix, &(result._matrix));
|
||||
return result;
|
||||
}
|
||||
inline void swapRows(size_t a, size_t b) {
|
||||
if (a == b) return;
|
||||
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
float tmp = (*this)(a, j);
|
||||
(*this)(a, j) = (*this)(b, j);
|
||||
(*this)(b, j) = tmp;
|
||||
}
|
||||
}
|
||||
inline void swapCols(size_t a, size_t b) {
|
||||
if (a == b) return;
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
float tmp = (*this)(i, a);
|
||||
(*this)(i, a) = (*this)(i, b);
|
||||
(*this)(i, b) = tmp;
|
||||
}
|
||||
}
|
||||
/**
|
||||
* inverse based on LU factorization with partial pivotting
|
||||
*/
|
||||
Matrix inverse() const {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(getRows() == getCols());
|
||||
#endif
|
||||
Matrix result(getRows(), getCols());
|
||||
Matrix work = (*this);
|
||||
arm_mat_inverse_f32(&(work._matrix),
|
||||
&(result._matrix));
|
||||
return result;
|
||||
}
|
||||
inline void setAll(const float &val) {
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
(*this)(i, j) = val;
|
||||
}
|
||||
}
|
||||
}
|
||||
inline void set(const float *data) {
|
||||
memcpy(getData(), data, getSize());
|
||||
}
|
||||
inline size_t getRows() const { return _matrix.numRows; }
|
||||
inline size_t getCols() const { return _matrix.numCols; }
|
||||
inline static Matrix identity(size_t size) {
|
||||
Matrix result(size, size);
|
||||
|
||||
for (size_t i = 0; i < size; i++) {
|
||||
result(i, i) = 1.0f;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
inline static Matrix zero(size_t size) {
|
||||
Matrix result(size, size);
|
||||
result.setAll(0.0f);
|
||||
return result;
|
||||
}
|
||||
inline static Matrix zero(size_t m, size_t n) {
|
||||
Matrix result(m, n);
|
||||
result.setAll(0.0f);
|
||||
return result;
|
||||
}
|
||||
protected:
|
||||
inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); }
|
||||
inline float *getData() { return _matrix.pData; }
|
||||
inline const float *getData() const { return _matrix.pData; }
|
||||
inline void setData(float *data) { _matrix.pData = data; }
|
||||
private:
|
||||
arm_matrix_instance_f32 _matrix;
|
||||
};
|
||||
|
||||
} // namespace math
|
|
@ -1,40 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Vector.cpp
|
||||
*
|
||||
* math vector
|
||||
*/
|
||||
|
||||
#include "Vector.hpp"
|
|
@ -1,236 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Vector.h
|
||||
*
|
||||
* math vector
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <assert.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "../Vector.hpp"
|
||||
#include "../test/test.hpp"
|
||||
|
||||
// arm specific
|
||||
#include "../../CMSIS/Include/arm_math.h"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
class __EXPORT Vector
|
||||
{
|
||||
public:
|
||||
// constructor
|
||||
Vector(size_t rows) :
|
||||
_rows(rows),
|
||||
_data((float *)calloc(rows, sizeof(float))) {
|
||||
}
|
||||
Vector(size_t rows, const float *data) :
|
||||
_rows(rows),
|
||||
_data((float *)malloc(getSize())) {
|
||||
memcpy(getData(), data, getSize());
|
||||
}
|
||||
// deconstructor
|
||||
virtual ~Vector() {
|
||||
delete [] getData();
|
||||
}
|
||||
// copy constructor (deep)
|
||||
Vector(const Vector &right) :
|
||||
_rows(right.getRows()),
|
||||
_data((float *)malloc(getSize())) {
|
||||
memcpy(getData(), right.getData(),
|
||||
right.getSize());
|
||||
}
|
||||
// assignment
|
||||
inline Vector &operator=(const Vector &right) {
|
||||
#ifdef VECTOR_ASSERT
|
||||
ASSERT(getRows() == right.getRows());
|
||||
#endif
|
||||
|
||||
if (this != &right) {
|
||||
memcpy(getData(), right.getData(),
|
||||
right.getSize());
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
// element accessors
|
||||
inline float &operator()(size_t i) {
|
||||
#ifdef VECTOR_ASSERT
|
||||
ASSERT(i < getRows());
|
||||
#endif
|
||||
return getData()[i];
|
||||
}
|
||||
inline const float &operator()(size_t i) const {
|
||||
#ifdef VECTOR_ASSERT
|
||||
ASSERT(i < getRows());
|
||||
#endif
|
||||
return getData()[i];
|
||||
}
|
||||
// output
|
||||
inline void print() const {
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
float sig;
|
||||
int exponent;
|
||||
float num = (*this)(i);
|
||||
float2SigExp(num, sig, exponent);
|
||||
printf("%6.3fe%03d ", (double)sig, exponent);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
}
|
||||
// boolean ops
|
||||
inline bool operator==(const Vector &right) const {
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
if (fabsf(((*this)(i) - right(i))) > 1e-30f)
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
// scalar ops
|
||||
inline Vector operator+(float right) const {
|
||||
Vector result(getRows());
|
||||
arm_offset_f32((float *)getData(),
|
||||
right, result.getData(),
|
||||
getRows());
|
||||
return result;
|
||||
}
|
||||
inline Vector operator-(float right) const {
|
||||
Vector result(getRows());
|
||||
arm_offset_f32((float *)getData(),
|
||||
-right, result.getData(),
|
||||
getRows());
|
||||
return result;
|
||||
}
|
||||
inline Vector operator*(float right) const {
|
||||
Vector result(getRows());
|
||||
arm_scale_f32((float *)getData(),
|
||||
right, result.getData(),
|
||||
getRows());
|
||||
return result;
|
||||
}
|
||||
inline Vector operator/(float right) const {
|
||||
Vector result(getRows());
|
||||
arm_scale_f32((float *)getData(),
|
||||
1.0f / right, result.getData(),
|
||||
getRows());
|
||||
return result;
|
||||
}
|
||||
// vector ops
|
||||
inline Vector operator+(const Vector &right) const {
|
||||
#ifdef VECTOR_ASSERT
|
||||
ASSERT(getRows() == right.getRows());
|
||||
#endif
|
||||
Vector result(getRows());
|
||||
arm_add_f32((float *)getData(),
|
||||
(float *)right.getData(),
|
||||
result.getData(),
|
||||
getRows());
|
||||
return result;
|
||||
}
|
||||
inline Vector operator-(const Vector &right) const {
|
||||
#ifdef VECTOR_ASSERT
|
||||
ASSERT(getRows() == right.getRows());
|
||||
#endif
|
||||
Vector result(getRows());
|
||||
arm_sub_f32((float *)getData(),
|
||||
(float *)right.getData(),
|
||||
result.getData(),
|
||||
getRows());
|
||||
return result;
|
||||
}
|
||||
inline Vector operator-(void) const {
|
||||
Vector result(getRows());
|
||||
arm_negate_f32((float *)getData(),
|
||||
result.getData(),
|
||||
getRows());
|
||||
return result;
|
||||
}
|
||||
// other functions
|
||||
inline float dot(const Vector &right) const {
|
||||
float result = 0;
|
||||
arm_dot_prod_f32((float *)getData(),
|
||||
(float *)right.getData(),
|
||||
getRows(),
|
||||
&result);
|
||||
return result;
|
||||
}
|
||||
inline float norm() const {
|
||||
return sqrtf(dot(*this));
|
||||
}
|
||||
inline float length() const {
|
||||
return norm();
|
||||
}
|
||||
inline Vector unit() const {
|
||||
return (*this) / norm();
|
||||
}
|
||||
inline Vector normalized() const {
|
||||
return unit();
|
||||
}
|
||||
inline void normalize() {
|
||||
(*this) = (*this) / norm();
|
||||
}
|
||||
inline static Vector zero(size_t rows) {
|
||||
Vector result(rows);
|
||||
// calloc returns zeroed memory
|
||||
return result;
|
||||
}
|
||||
inline void setAll(const float &val) {
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
(*this)(i) = val;
|
||||
}
|
||||
}
|
||||
inline void set(const float *data) {
|
||||
memcpy(getData(), data, getSize());
|
||||
}
|
||||
inline size_t getRows() const { return _rows; }
|
||||
inline const float *getData() const { return _data; }
|
||||
protected:
|
||||
inline size_t getSize() const { return sizeof(float) * getRows(); }
|
||||
inline float *getData() { return _data; }
|
||||
inline void setData(float *data) { _data = data; }
|
||||
private:
|
||||
size_t _rows;
|
||||
float *_data;
|
||||
};
|
||||
|
||||
} // math
|
|
@ -1,437 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Matrix.h
|
||||
*
|
||||
* matrix code
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <assert.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "../Vector.hpp"
|
||||
#include "../Matrix.hpp"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
class __EXPORT Matrix
|
||||
{
|
||||
public:
|
||||
// constructor
|
||||
Matrix(size_t rows, size_t cols) :
|
||||
_rows(rows),
|
||||
_cols(cols),
|
||||
_data((float *)calloc(rows *cols, sizeof(float))) {
|
||||
}
|
||||
Matrix(size_t rows, size_t cols, const float *data) :
|
||||
_rows(rows),
|
||||
_cols(cols),
|
||||
_data((float *)malloc(getSize())) {
|
||||
memcpy(getData(), data, getSize());
|
||||
}
|
||||
// deconstructor
|
||||
virtual ~Matrix() {
|
||||
delete [] getData();
|
||||
}
|
||||
// copy constructor (deep)
|
||||
Matrix(const Matrix &right) :
|
||||
_rows(right.getRows()),
|
||||
_cols(right.getCols()),
|
||||
_data((float *)malloc(getSize())) {
|
||||
memcpy(getData(), right.getData(),
|
||||
right.getSize());
|
||||
}
|
||||
// assignment
|
||||
inline Matrix &operator=(const Matrix &right) {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(getRows() == right.getRows());
|
||||
ASSERT(getCols() == right.getCols());
|
||||
#endif
|
||||
|
||||
if (this != &right) {
|
||||
memcpy(getData(), right.getData(),
|
||||
right.getSize());
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
// element accessors
|
||||
inline float &operator()(size_t i, size_t j) {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(i < getRows());
|
||||
ASSERT(j < getCols());
|
||||
#endif
|
||||
return getData()[i * getCols() + j];
|
||||
}
|
||||
inline const float &operator()(size_t i, size_t j) const {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(i < getRows());
|
||||
ASSERT(j < getCols());
|
||||
#endif
|
||||
return getData()[i * getCols() + j];
|
||||
}
|
||||
// output
|
||||
inline void print() const {
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
float sig;
|
||||
int exp;
|
||||
float num = (*this)(i, j);
|
||||
float2SigExp(num, sig, exp);
|
||||
printf("%6.3fe%03.3d,", (double)sig, exp);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
// boolean ops
|
||||
inline bool operator==(const Matrix &right) const {
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
// scalar ops
|
||||
inline Matrix operator+(const float &right) const {
|
||||
Matrix result(getRows(), getCols());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
result(i, j) = (*this)(i, j) + right;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
inline Matrix operator-(const float &right) const {
|
||||
Matrix result(getRows(), getCols());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
result(i, j) = (*this)(i, j) - right;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
inline Matrix operator*(const float &right) const {
|
||||
Matrix result(getRows(), getCols());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
result(i, j) = (*this)(i, j) * right;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
inline Matrix operator/(const float &right) const {
|
||||
Matrix result(getRows(), getCols());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
result(i, j) = (*this)(i, j) / right;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
// vector ops
|
||||
inline Vector operator*(const Vector &right) const {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(getCols() == right.getRows());
|
||||
#endif
|
||||
Vector result(getRows());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
result(i) += (*this)(i, j) * right(j);
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
// matrix ops
|
||||
inline Matrix operator+(const Matrix &right) const {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(getRows() == right.getRows());
|
||||
ASSERT(getCols() == right.getCols());
|
||||
#endif
|
||||
Matrix result(getRows(), getCols());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
result(i, j) = (*this)(i, j) + right(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
inline Matrix operator-(const Matrix &right) const {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(getRows() == right.getRows());
|
||||
ASSERT(getCols() == right.getCols());
|
||||
#endif
|
||||
Matrix result(getRows(), getCols());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
result(i, j) = (*this)(i, j) - right(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
inline Matrix operator*(const Matrix &right) const {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(getCols() == right.getRows());
|
||||
#endif
|
||||
Matrix result(getRows(), right.getCols());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < right.getCols(); j++) {
|
||||
for (size_t k = 0; k < right.getRows(); k++) {
|
||||
result(i, j) += (*this)(i, k) * right(k, j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
inline Matrix operator/(const Matrix &right) const {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(right.getRows() == right.getCols());
|
||||
ASSERT(getCols() == right.getCols());
|
||||
#endif
|
||||
return (*this) * right.inverse();
|
||||
}
|
||||
// other functions
|
||||
inline Matrix transpose() const {
|
||||
Matrix result(getCols(), getRows());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
result(j, i) = (*this)(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
inline void swapRows(size_t a, size_t b) {
|
||||
if (a == b) return;
|
||||
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
float tmp = (*this)(a, j);
|
||||
(*this)(a, j) = (*this)(b, j);
|
||||
(*this)(b, j) = tmp;
|
||||
}
|
||||
}
|
||||
inline void swapCols(size_t a, size_t b) {
|
||||
if (a == b) return;
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
float tmp = (*this)(i, a);
|
||||
(*this)(i, a) = (*this)(i, b);
|
||||
(*this)(i, b) = tmp;
|
||||
}
|
||||
}
|
||||
/**
|
||||
* inverse based on LU factorization with partial pivotting
|
||||
*/
|
||||
Matrix inverse() const {
|
||||
#ifdef MATRIX_ASSERT
|
||||
ASSERT(getRows() == getCols());
|
||||
#endif
|
||||
size_t N = getRows();
|
||||
Matrix L = identity(N);
|
||||
const Matrix &A = (*this);
|
||||
Matrix U = A;
|
||||
Matrix P = identity(N);
|
||||
|
||||
//printf("A:\n"); A.print();
|
||||
|
||||
// for all diagonal elements
|
||||
for (size_t n = 0; n < N; n++) {
|
||||
|
||||
// if diagonal is zero, swap with row below
|
||||
if (fabsf(U(n, n)) < 1e-8f) {
|
||||
//printf("trying pivot for row %d\n",n);
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
if (i == n) continue;
|
||||
|
||||
//printf("\ttrying row %d\n",i);
|
||||
if (fabsf(U(i, n)) > 1e-8f) {
|
||||
//printf("swapped %d\n",i);
|
||||
U.swapRows(i, n);
|
||||
P.swapRows(i, n);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef MATRIX_ASSERT
|
||||
//printf("A:\n"); A.print();
|
||||
//printf("U:\n"); U.print();
|
||||
//printf("P:\n"); P.print();
|
||||
//fflush(stdout);
|
||||
ASSERT(fabsf(U(n, n)) > 1e-8f);
|
||||
#endif
|
||||
|
||||
// failsafe, return zero matrix
|
||||
if (fabsf(U(n, n)) < 1e-8f) {
|
||||
return Matrix::zero(n);
|
||||
}
|
||||
|
||||
// for all rows below diagonal
|
||||
for (size_t i = (n + 1); i < N; i++) {
|
||||
L(i, n) = U(i, n) / U(n, n);
|
||||
|
||||
// add i-th row and n-th row
|
||||
// multiplied by: -a(i,n)/a(n,n)
|
||||
for (size_t k = n; k < N; k++) {
|
||||
U(i, k) -= L(i, n) * U(n, k);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//printf("L:\n"); L.print();
|
||||
//printf("U:\n"); U.print();
|
||||
|
||||
// solve LY=P*I for Y by forward subst
|
||||
Matrix Y = P;
|
||||
|
||||
// for all columns of Y
|
||||
for (size_t c = 0; c < N; c++) {
|
||||
// for all rows of L
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
// for all columns of L
|
||||
for (size_t j = 0; j < i; j++) {
|
||||
// for all existing y
|
||||
// subtract the component they
|
||||
// contribute to the solution
|
||||
Y(i, c) -= L(i, j) * Y(j, c);
|
||||
}
|
||||
|
||||
// divide by the factor
|
||||
// on current
|
||||
// term to be solved
|
||||
// Y(i,c) /= L(i,i);
|
||||
// but L(i,i) = 1.0
|
||||
}
|
||||
}
|
||||
|
||||
//printf("Y:\n"); Y.print();
|
||||
|
||||
// solve Ux=y for x by back subst
|
||||
Matrix X = Y;
|
||||
|
||||
// for all columns of X
|
||||
for (size_t c = 0; c < N; c++) {
|
||||
// for all rows of U
|
||||
for (size_t k = 0; k < N; k++) {
|
||||
// have to go in reverse order
|
||||
size_t i = N - 1 - k;
|
||||
|
||||
// for all columns of U
|
||||
for (size_t j = i + 1; j < N; j++) {
|
||||
// for all existing x
|
||||
// subtract the component they
|
||||
// contribute to the solution
|
||||
X(i, c) -= U(i, j) * X(j, c);
|
||||
}
|
||||
|
||||
// divide by the factor
|
||||
// on current
|
||||
// term to be solved
|
||||
X(i, c) /= U(i, i);
|
||||
}
|
||||
}
|
||||
|
||||
//printf("X:\n"); X.print();
|
||||
return X;
|
||||
}
|
||||
inline void setAll(const float &val) {
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
for (size_t j = 0; j < getCols(); j++) {
|
||||
(*this)(i, j) = val;
|
||||
}
|
||||
}
|
||||
}
|
||||
inline void set(const float *data) {
|
||||
memcpy(getData(), data, getSize());
|
||||
}
|
||||
inline size_t getRows() const { return _rows; }
|
||||
inline size_t getCols() const { return _cols; }
|
||||
inline static Matrix identity(size_t size) {
|
||||
Matrix result(size, size);
|
||||
|
||||
for (size_t i = 0; i < size; i++) {
|
||||
result(i, i) = 1.0f;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
inline static Matrix zero(size_t size) {
|
||||
Matrix result(size, size);
|
||||
result.setAll(0.0f);
|
||||
return result;
|
||||
}
|
||||
inline static Matrix zero(size_t m, size_t n) {
|
||||
Matrix result(m, n);
|
||||
result.setAll(0.0f);
|
||||
return result;
|
||||
}
|
||||
protected:
|
||||
inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); }
|
||||
inline float *getData() { return _data; }
|
||||
inline const float *getData() const { return _data; }
|
||||
inline void setData(float *data) { _data = data; }
|
||||
private:
|
||||
size_t _rows;
|
||||
size_t _cols;
|
||||
float *_data;
|
||||
};
|
||||
|
||||
} // namespace math
|
|
@ -1,40 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Vector.cpp
|
||||
*
|
||||
* math vector
|
||||
*/
|
||||
|
||||
#include "Vector.hpp"
|
|
@ -1,245 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Vector.h
|
||||
*
|
||||
* math vector
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <assert.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "../Vector.hpp"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
class __EXPORT Vector
|
||||
{
|
||||
public:
|
||||
// constructor
|
||||
Vector(size_t rows) :
|
||||
_rows(rows),
|
||||
_data((float *)calloc(rows, sizeof(float))) {
|
||||
}
|
||||
Vector(size_t rows, const float *data) :
|
||||
_rows(rows),
|
||||
_data((float *)malloc(getSize())) {
|
||||
memcpy(getData(), data, getSize());
|
||||
}
|
||||
// deconstructor
|
||||
virtual ~Vector() {
|
||||
delete [] getData();
|
||||
}
|
||||
// copy constructor (deep)
|
||||
Vector(const Vector &right) :
|
||||
_rows(right.getRows()),
|
||||
_data((float *)malloc(getSize())) {
|
||||
memcpy(getData(), right.getData(),
|
||||
right.getSize());
|
||||
}
|
||||
// assignment
|
||||
inline Vector &operator=(const Vector &right) {
|
||||
#ifdef VECTOR_ASSERT
|
||||
ASSERT(getRows() == right.getRows());
|
||||
#endif
|
||||
|
||||
if (this != &right) {
|
||||
memcpy(getData(), right.getData(),
|
||||
right.getSize());
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
// element accessors
|
||||
inline float &operator()(size_t i) {
|
||||
#ifdef VECTOR_ASSERT
|
||||
ASSERT(i < getRows());
|
||||
#endif
|
||||
return getData()[i];
|
||||
}
|
||||
inline const float &operator()(size_t i) const {
|
||||
#ifdef VECTOR_ASSERT
|
||||
ASSERT(i < getRows());
|
||||
#endif
|
||||
return getData()[i];
|
||||
}
|
||||
// output
|
||||
inline void print() const {
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
float sig;
|
||||
int exp;
|
||||
float num = (*this)(i);
|
||||
float2SigExp(num, sig, exp);
|
||||
printf("%6.3fe%03.3d,", (double)sig, exp);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
}
|
||||
// boolean ops
|
||||
inline bool operator==(const Vector &right) const {
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
if (fabsf(((*this)(i) - right(i))) > 1e-30f)
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
// scalar ops
|
||||
inline Vector operator+(const float &right) const {
|
||||
Vector result(getRows());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
result(i) = (*this)(i) + right;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
inline Vector operator-(const float &right) const {
|
||||
Vector result(getRows());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
result(i) = (*this)(i) - right;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
inline Vector operator*(const float &right) const {
|
||||
Vector result(getRows());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
result(i) = (*this)(i) * right;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
inline Vector operator/(const float &right) const {
|
||||
Vector result(getRows());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
result(i) = (*this)(i) / right;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
// vector ops
|
||||
inline Vector operator+(const Vector &right) const {
|
||||
#ifdef VECTOR_ASSERT
|
||||
ASSERT(getRows() == right.getRows());
|
||||
#endif
|
||||
Vector result(getRows());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
result(i) = (*this)(i) + right(i);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
inline Vector operator-(const Vector &right) const {
|
||||
#ifdef VECTOR_ASSERT
|
||||
ASSERT(getRows() == right.getRows());
|
||||
#endif
|
||||
Vector result(getRows());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
result(i) = (*this)(i) - right(i);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
inline Vector operator-(void) const {
|
||||
Vector result(getRows());
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
result(i) = -((*this)(i));
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
// other functions
|
||||
inline float dot(const Vector &right) const {
|
||||
float result = 0;
|
||||
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
result += (*this)(i) * (*this)(i);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
inline float norm() const {
|
||||
return sqrtf(dot(*this));
|
||||
}
|
||||
inline float length() const {
|
||||
return norm();
|
||||
}
|
||||
inline Vector unit() const {
|
||||
return (*this) / norm();
|
||||
}
|
||||
inline Vector normalized() const {
|
||||
return unit();
|
||||
}
|
||||
inline void normalize() {
|
||||
(*this) = (*this) / norm();
|
||||
}
|
||||
inline static Vector zero(size_t rows) {
|
||||
Vector result(rows);
|
||||
// calloc returns zeroed memory
|
||||
return result;
|
||||
}
|
||||
inline void setAll(const float &val) {
|
||||
for (size_t i = 0; i < getRows(); i++) {
|
||||
(*this)(i) = val;
|
||||
}
|
||||
}
|
||||
inline void set(const float *data) {
|
||||
memcpy(getData(), data, getSize());
|
||||
}
|
||||
inline size_t getRows() const { return _rows; }
|
||||
protected:
|
||||
inline size_t getSize() const { return sizeof(float) * getRows(); }
|
||||
inline float *getData() { return _data; }
|
||||
inline const float *getData() const { return _data; }
|
||||
inline void setData(float *data) { _data = data; }
|
||||
private:
|
||||
size_t _rows;
|
||||
float *_data;
|
||||
};
|
||||
|
||||
} // math
|
|
@ -41,13 +41,12 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "math/Dcm.hpp"
|
||||
#include "math/EulerAngles.hpp"
|
||||
#include "math/Matrix.hpp"
|
||||
#include "math/Quaternion.hpp"
|
||||
#include "math/Vector.hpp"
|
||||
#include "math/Vector2.hpp"
|
||||
#include "math/Vector3.hpp"
|
||||
#include "math/Vector2f.hpp"
|
||||
#include "math/Matrix.hpp"
|
||||
#include "math/Matrix3.hpp"
|
||||
#include "math/Quaternion.hpp"
|
||||
#include "math/Limits.hpp"
|
||||
|
||||
#endif
|
||||
|
@ -56,4 +55,4 @@
|
|||
|
||||
#include "CMSIS/Include/arm_math.h"
|
||||
|
||||
#endif
|
||||
#endif
|
||||
|
|
|
@ -35,13 +35,7 @@
|
|||
# Math library
|
||||
#
|
||||
SRCS = math/test/test.cpp \
|
||||
math/Vector.cpp \
|
||||
math/Vector2f.cpp \
|
||||
math/Vector3.cpp \
|
||||
math/EulerAngles.cpp \
|
||||
math/Quaternion.cpp \
|
||||
math/Dcm.cpp \
|
||||
math/Matrix.cpp \
|
||||
math/Matrix3.cpp \
|
||||
math/Limits.cpp
|
||||
|
||||
#
|
||||
|
@ -49,13 +43,3 @@ SRCS = math/test/test.cpp \
|
|||
# current makefile name, since app.mk needs it.
|
||||
#
|
||||
APP_MAKEFILE := $(lastword $(MAKEFILE_LIST))
|
||||
|
||||
ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy)
|
||||
INCLUDE_DIRS += math/arm
|
||||
SRCS += math/arm/Vector.cpp \
|
||||
math/arm/Matrix.cpp
|
||||
else
|
||||
#INCLUDE_DIRS += math/generic
|
||||
#SRCS += math/generic/Vector.cpp \
|
||||
# math/generic/Matrix.cpp
|
||||
endif
|
||||
|
|
|
@ -24,6 +24,7 @@ SRCS = test_adc.c \
|
|||
test_uart_loopback.c \
|
||||
test_uart_send.c \
|
||||
test_mixer.cpp \
|
||||
test_mathlib.cpp \
|
||||
test_file.c \
|
||||
tests_main.c \
|
||||
test_param.c \
|
||||
|
|
|
@ -32,72 +32,83 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Vector2f.cpp
|
||||
* @file test_mathlib.cpp
|
||||
*
|
||||
* math vector
|
||||
* Mathlib test
|
||||
*/
|
||||
|
||||
#include "test/test.hpp"
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include "Vector2f.hpp"
|
||||
#include "tests.h"
|
||||
|
||||
namespace math
|
||||
{
|
||||
using namespace math;
|
||||
|
||||
Vector2f::Vector2f() :
|
||||
Vector(2)
|
||||
{
|
||||
const char* formatResult(bool res) {
|
||||
return res ? "OK" : "ERROR";
|
||||
}
|
||||
|
||||
Vector2f::Vector2f(const Vector &right) :
|
||||
Vector(right)
|
||||
int test_mathlib(int argc, char *argv[])
|
||||
{
|
||||
#ifdef VECTOR_ASSERT
|
||||
ASSERT(right.getRows() == 2);
|
||||
#endif
|
||||
}
|
||||
warnx("testing mathlib");
|
||||
|
||||
Vector2f::Vector2f(float x, float y) :
|
||||
Vector(2)
|
||||
{
|
||||
setX(x);
|
||||
setY(y);
|
||||
}
|
||||
Matrix3f m;
|
||||
m.identity();
|
||||
Matrix3f m1;
|
||||
Matrix<3,3> mq;
|
||||
mq.identity();
|
||||
Matrix<3,3> mq1;
|
||||
m1(0, 0) = 5.0;
|
||||
Vector3f v = Vector3f(1.0f, 2.0f, 3.0f);
|
||||
Vector3f v1;
|
||||
|
||||
Vector2f::Vector2f(const float *data) :
|
||||
Vector(2, data)
|
||||
{
|
||||
}
|
||||
unsigned int n = 60000;
|
||||
|
||||
Vector2f::~Vector2f()
|
||||
{
|
||||
}
|
||||
hrt_abstime t0, t1;
|
||||
|
||||
float Vector2f::cross(const Vector2f &b) const
|
||||
{
|
||||
const Vector2f &a = *this;
|
||||
return a(0)*b(1) - a(1)*b(0);
|
||||
}
|
||||
t0 = hrt_absolute_time();
|
||||
for (unsigned int j = 0; j < n; j++) {
|
||||
v1 = m * v;
|
||||
}
|
||||
t1 = hrt_absolute_time();
|
||||
warnx("Matrix * Vector: %s %.6fus", formatResult(v1 == v), (double)(t1 - t0) / n);
|
||||
|
||||
float Vector2f::operator %(const Vector2f &v) const
|
||||
{
|
||||
return cross(v);
|
||||
}
|
||||
|
||||
float Vector2f::operator *(const Vector2f &v) const
|
||||
{
|
||||
return dot(v);
|
||||
}
|
||||
t0 = hrt_absolute_time();
|
||||
for (unsigned int j = 0; j < n; j++) {
|
||||
mq1 = mq * mq;
|
||||
}
|
||||
t1 = hrt_absolute_time();
|
||||
warnx("Matrix * Matrix: %s %.6fus", formatResult(mq1 == mq), (double)(t1 - t0) / n);
|
||||
mq1.dump();
|
||||
|
||||
int __EXPORT vector2fTest()
|
||||
{
|
||||
printf("Test Vector2f\t\t: ");
|
||||
// test float ctor
|
||||
Vector2f v(1, 2);
|
||||
ASSERT(equal(v(0), 1));
|
||||
ASSERT(equal(v(1), 2));
|
||||
printf("PASS\n");
|
||||
t0 = hrt_absolute_time();
|
||||
for (unsigned int j = 0; j < n; j++) {
|
||||
m1 = m.transposed();
|
||||
}
|
||||
t1 = hrt_absolute_time();
|
||||
warnx("Matrix Transpose: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n);
|
||||
|
||||
t0 = hrt_absolute_time();
|
||||
for (unsigned int j = 0; j < n; j++) {
|
||||
m1 = m.inversed();
|
||||
}
|
||||
t1 = hrt_absolute_time();
|
||||
warnx("Matrix Invert: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n);
|
||||
|
||||
Matrix<4,4> mn;
|
||||
mn(0, 0) = 2.0f;
|
||||
mn(1, 0) = 3.0f;
|
||||
for (int i = 0; i < mn.getRows(); i++) {
|
||||
for (int j = 0; j < mn.getCols(); j++) {
|
||||
printf("%.3f ", mn(i, j));
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // namespace math
|
|
@ -108,6 +108,7 @@ extern int test_param(int argc, char *argv[]);
|
|||
extern int test_bson(int argc, char *argv[]);
|
||||
extern int test_file(int argc, char *argv[]);
|
||||
extern int test_mixer(int argc, char *argv[]);
|
||||
extern int test_mathlib(int argc, char *argv[]);
|
||||
|
||||
__END_DECLS
|
||||
|
||||
|
|
|
@ -105,6 +105,7 @@ const struct {
|
|||
{"bson", test_bson, 0},
|
||||
{"file", test_file, 0},
|
||||
{"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST},
|
||||
{"mathlib", test_mathlib, 0},
|
||||
{"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
|
||||
{NULL, NULL, 0}
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue