dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Added math library.

This commit is contained in:
jgoppert 2013-01-06 14:08:50 -05:00
parent 1579630efe
commit db3fabc3ba
22 changed files with 2834 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
apps/examples/math_demo/Makefile Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# 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.
#
############################################################################
#
# Basic example application
#
APPNAME = math_demo
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 8192
include $(APPDIR)/mk/app.mk

110
apps/examples/math_demo/math_demo.cpp Normal file
View File

@ -0,0 +1,110 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.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 math_demo.cpp
* Demonstration of math library
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <systemlib/math/Vector.hpp>
#include <systemlib/math/Matrix.hpp>
#include <systemlib/math/Quaternion.hpp>
#include <systemlib/math/Vector3.hpp>
#include <systemlib/math/Dcm.hpp>
#include <systemlib/math/EulerAngles.hpp>
/**
* Management function.
*/
extern "C" __EXPORT int math_demo_main(int argc, char *argv[]);
/**
* Test function
*/
void test();
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: math_demo {test}\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int math_demo_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "test")) {
test();
exit(0);
}
usage("unrecognized command");
exit(1);
}
void test()
{
printf("beginning math lib test\n");
using namespace math;
vectorTest();
matrixTest();
vector3Test();
eulerAnglesTest();
quaternionTest();
dcmTest();
}

136
apps/systemlib/math/Dcm.cpp Normal file
View File

@ -0,0 +1,136 @@
/****************************************************************************
*
* 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 <systemlib/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(const float * data) :
Matrix(3,3,data)
{
}
Dcm::Dcm(const Quaternion & q) :
Matrix(3,3)
{
Dcm & dcm = *this;
float a = q.getA();
float b = q.getB();
float c = q.getC();
float d = q.getD();
float aSq = a*a;
float bSq = b*b;
float cSq = c*c;
float dSq = d*d;
dcm(0,0) = aSq + bSq - cSq - dSq;
dcm(0,1) = 2*(b*c - a*d);
dcm(0,2) = 2*(a*c + b*d);
dcm(1,0) = 2*(b*c + a*d);
dcm(1,1) = aSq - bSq + cSq - dSq;
dcm(1,2) = 2*(c*d - a*b);
dcm(2,0) = 2*(b*d - a*c);
dcm(2,1) = 2*(a*b + c*d);
dcm(2,2) = aSq - bSq - cSq + dSq;
}
Dcm::Dcm(const EulerAngles & euler) :
Matrix(3,3)
{
Dcm & dcm = *this;
float cosPhi = cosf(euler.getPhi());
float sinPhi = sinf(euler.getPhi());
float cosThe = cosf(euler.getTheta());
float sinThe = sinf(euler.getTheta());
float cosPsi = cosf(euler.getPsi());
float sinPsi = sinf(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: ");
Vector3 vB(1,2,3);
ASSERT(matrixEqual(Dcm(Quaternion(1,0,0,0)),
Matrix::identity(3)));
ASSERT(matrixEqual(Dcm(EulerAngles(0,0,0)),
Matrix::identity(3)));
ASSERT(vectorEqual(Vector3(-2,1,3),
Dcm(EulerAngles(0,0,M_PI_2_F))*vB));
ASSERT(vectorEqual(Vector3(3,2,-1),
Dcm(EulerAngles(0,M_PI_2_F,0))*vB));
ASSERT(vectorEqual(Vector3(1,-3,2),
Dcm(EulerAngles(M_PI_2_F,0,0))*vB));
ASSERT(vectorEqual(Vector3(3,2,-1),
Dcm(EulerAngles(
M_PI_2_F,M_PI_2_F,M_PI_2_F))*vB));
printf("PASS\n");
return 0;
}
} // namespace math

95
apps/systemlib/math/Dcm.hpp Normal file
View File

@ -0,0 +1,95 @@
/****************************************************************************
*
* 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();
/**
* data ctor
*/
Dcm(const float * data);
/**
* 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

124
apps/systemlib/math/EulerAngles.cpp Normal file
View File

@ -0,0 +1,124 @@
/****************************************************************************
*
* 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 <systemlib/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(1,2,3);
// test ctor
ASSERT(vectorEqual(Vector3(1,2,3),euler));
ASSERT(equal(euler.getPhi(),1));
ASSERT(equal(euler.getTheta(),2));
ASSERT(equal(euler.getPsi(),3));
// test dcm ctor
// test assignment
euler.setPhi(4);
ASSERT(equal(euler.getPhi(),4));
euler.setTheta(5);
ASSERT(equal(euler.getTheta(),5));
euler.setPsi(6);
ASSERT(equal(euler.getPsi(),6));
printf("PASS\n");
return 0;
}
} // namespace math

73
apps/systemlib/math/EulerAngles.hpp Normal file
View File

@ -0,0 +1,73 @@
/****************************************************************************
*
* 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

180
apps/systemlib/math/Matrix.cpp Normal file
View File

@ -0,0 +1,180 @@
/****************************************************************************
*
* 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 <systemlib/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

58
apps/systemlib/math/Matrix.hpp Normal file
View File

@ -0,0 +1,58 @@
/****************************************************************************
*
* 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
#ifdef ARM_MATH_CM4
#include "arm/Matrix.hpp"
#else
#include "generic/Matrix.hpp"
#endif
namespace math {
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

180
apps/systemlib/math/Quaternion.cpp Normal file
View File

@ -0,0 +1,180 @@
/****************************************************************************
*
* 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 <systemlib/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)
{
setA(0.5f*sqrtf(1 + dcm(0,0) +
dcm(1,1) + dcm(2,2)));
setB((dcm(2,1) - dcm(1,2))/
(4*getA()));
setC((dcm(0,2) - dcm(2,0))/
(4*getA()));
setD((dcm(1,0) - dcm(0,1))/
(4*getA()));
}
Quaternion::Quaternion(const EulerAngles & euler) :
Vector(4)
{
float cosPhi_2 = cosf(euler.getPhi()/2.0f);
float cosTheta_2 = cosf(euler.getTheta()/2.0f);
float cosPsi_2 = cosf(euler.getPsi()/2.0f);
float sinPhi_2 = sinf(euler.getPhi()/2.0f);
float sinTheta_2 = sinf(euler.getTheta()/2.0f);
float sinPsi_2 = sinf(euler.getPsi()/2.0f);
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));
ASSERT(equal(q.getB(),0));
ASSERT(equal(q.getC(),0));
ASSERT(equal(q.getD(),0));
// test float ctor
q = Quaternion(0,1,0,0);
ASSERT(equal(q.getA(),0));
ASSERT(equal(q.getB(),1));
ASSERT(equal(q.getC(),0));
ASSERT(equal(q.getD(),0));
// test euler ctor
q = Quaternion(EulerAngles(0,0,0));
ASSERT(equal(q.getA(),1));
ASSERT(equal(q.getB(),0));
ASSERT(equal(q.getC(),0));
ASSERT(equal(q.getD(),0));
// test dcm ctor
q = Quaternion(Dcm());
ASSERT(equal(q.getA(),1));
ASSERT(equal(q.getB(),0));
ASSERT(equal(q.getC(),0));
ASSERT(equal(q.getD(),0));
// TODO test derivative
// test accessors
q.setA(0.1);
q.setB(0.2);
q.setC(0.3);
q.setD(0.4);
ASSERT(equal(q.getA(),0.1));
ASSERT(equal(q.getB(),0.2));
ASSERT(equal(q.getC(),0.3));
ASSERT(equal(q.getD(),0.4));
printf("PASS\n");
return 0;
}
} // namespace math

114
apps/systemlib/math/Quaternion.hpp Normal file
View File

@ -0,0 +1,114 @@
/****************************************************************************
*
* 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.hpp
*
* math quaternion lib
*/
//#pragma once
#include "Vector.hpp"
#include "Matrix.hpp"
namespace math {
class Dcm;
class EulerAngles;
class __EXPORT Quaternion : public Vector
{
public:
/**
* default ctor
*/
Quaternion();
/**
* ctor from floats
*/
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); }
};
int __EXPORT quaternionTest();
} // math

99
apps/systemlib/math/Vector.cpp Normal file
View File

@ -0,0 +1,99 @@
/****************************************************************************
*
* 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 <systemlib/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

54
apps/systemlib/math/Vector.hpp Normal file
View File

@ -0,0 +1,54 @@
/****************************************************************************
*
* 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
#ifdef ARM_MATH_CM4
#include "arm/Vector.hpp"
#else
#include "generic/Vector.hpp"
#endif
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

99
apps/systemlib/math/Vector3.cpp Normal file
View File

@ -0,0 +1,99 @@
/****************************************************************************
*
* 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 <systemlib/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)
{
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

71
apps/systemlib/math/Vector3.hpp Normal file
View File

@ -0,0 +1,71 @@
/****************************************************************************
*
* 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.hpp
*
* math 3 vector
*/
#pragma once
#include "Vector.hpp"
namespace math
{
class __EXPORT Vector3 :
public Vector
{
public:
Vector3();
Vector3(const Vector & right);
Vector3(float x, float y, float z);
Vector3(const float * data);
virtual ~Vector3();
Vector3 cross(const Vector3 & b);
/**
* accessors
*/
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); }
};
int __EXPORT vector3Test();
} // math

40
apps/systemlib/math/arm/Matrix.cpp Normal file
View File

@ -0,0 +1,40 @@
/****************************************************************************
*
* 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"

312
apps/systemlib/math/arm/Matrix.hpp Normal file
View File

@ -0,0 +1,312 @@
/****************************************************************************
*
* 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 <systemlib/math/Vector.hpp>
#include <systemlib/math/Matrix.hpp>
// arm specific
#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 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+(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

40
apps/systemlib/math/arm/Vector.cpp Normal file
View File

@ -0,0 +1,40 @@
/****************************************************************************
*
* 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"

240
apps/systemlib/math/arm/Vector.hpp Normal file
View File

@ -0,0 +1,240 @@
/****************************************************************************
*
* 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 <systemlib/math/Vector.hpp>
#include <systemlib/test/test.hpp>
// arm specific
#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 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+(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;
}
// other functions
inline float dot(const Vector & right)
{
float result = 0;
arm_dot_prod_f32((float*)getData(),
(float*)right.getData(),
getRows(),
&result);
return result;
}
inline float norm()
{
return sqrtf(dot(*this));
}
inline Vector unit()
{
return (*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

40
apps/systemlib/math/generic/Matrix.cpp Normal file
View File

@ -0,0 +1,40 @@
/****************************************************************************
*
* 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"

447
apps/systemlib/math/generic/Matrix.hpp Normal file
View File

@ -0,0 +1,447 @@
/****************************************************************************
*
* 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 <systemlib/math/Vector.hpp>
#include <systemlib/math/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

40
apps/systemlib/math/generic/Vector.cpp Normal file
View File

@ -0,0 +1,40 @@
/****************************************************************************
*
* 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"

240
apps/systemlib/math/generic/Vector.hpp Normal file
View File

@ -0,0 +1,240 @@
/****************************************************************************
*
* 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 <systemlib/math/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;
}
// other functions
inline float dot(const Vector & right)
{
float result = 0;
for (size_t i=0; i<getRows(); i++)
{
result += (*this)(i)*(*this)(i);
}
return result;
}
inline float norm()
{
return sqrtf(dot(*this));
}
inline Vector unit()
{
return (*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