git-svn-id: svn://db.shs.com.ru/pip@290 12ceb7fc-bf1f-11e4-8940-5bc7170c53b5
This commit is contained in:
@@ -25,22 +25,22 @@ macro (pip_code_model RESULT)
|
|||||||
set(CCM_OPTS "${it}")
|
set(CCM_OPTS "${it}")
|
||||||
endif()
|
endif()
|
||||||
endforeach()
|
endforeach()
|
||||||
message(STATUS "src = ${CCM_SRC}")
|
#message(STATUS "src = ${CCM_SRC}")
|
||||||
#message(STATUS "result = ${RESULT}")
|
#message(STATUS "result = ${RESULT}")
|
||||||
message(STATUS "options = ${CCM_OPTS}")
|
#message(STATUS "options = ${CCM_OPTS}")
|
||||||
set(CCM_OUT ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_ccm.cpp)
|
set(CCM_OUT ${CMAKE_CURRENT_BINARY_DIR}/ccm_${PROJECT_NAME}.cpp)
|
||||||
set(${RESULT} ${${RESULT}} ${CCM_OUT})
|
set(${RESULT} ${${RESULT}} ${CCM_OUT})
|
||||||
message(STATUS "CCM = ${RESULT}")
|
#message(STATUS "CCM = ${RESULT}")
|
||||||
add_custom_command(OUTPUT ${CCM_OUT}
|
add_custom_command(OUTPUT ${CCM_OUT}
|
||||||
COMMAND ${PIP_CMG}
|
COMMAND ${PIP_CMG}
|
||||||
ARGS -P ${CCM_OPTS} -o ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_ccm -I${PIP_INCLUDES} ${CMAKE_CURRENT_SOURCE_DIR}/${CCM_SRC}
|
ARGS -P ${CCM_OPTS} -o ${CMAKE_CURRENT_BINARY_DIR}/ccm_${PROJECT_NAME} -I${PIP_INCLUDES} ${CMAKE_CURRENT_SOURCE_DIR}/${CCM_SRC}
|
||||||
MAIN_DEPENDENCY ${CCM_SRC})
|
MAIN_DEPENDENCY ${CCM_SRC})
|
||||||
#VERBATIM)
|
#VERBATIM)
|
||||||
#message(STATUS "Creating code model based on \"${CCM_SRC}\", please wait ... ")
|
#message(STATUS "Creating code model based on \"${CCM_SRC}\", please wait ... ")
|
||||||
#message(STATUS "exec \"-qP ${CCM_OPTS} -o ${PROJECT_NAME}_ccm -I${PIP_INCLUDES} ${CCM_SRC}\"")
|
#message(STATUS "exec \"-qP ${CCM_OPTS} -o ccm_${PROJECT_NAME} -I${PIP_INCLUDES} ${CCM_SRC}\"")
|
||||||
message(STATUS "exec ${PIP_CMG} -P ${CCM_OPTS} -o ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_ccm -I${PIP_INCLUDES} ${CMAKE_CURRENT_SOURCE_DIR}/${CCM_SRC}")
|
message(STATUS "Run CCM: ${PIP_CMG} -P ${CCM_OPTS} -o ${CMAKE_CURRENT_BINARY_DIR}/ccm_${PROJECT_NAME} -I${PIP_INCLUDES} ${CMAKE_CURRENT_SOURCE_DIR}/${CCM_SRC}")
|
||||||
#execute_process(OUTPUT_VARIABLE CMG_OUT COMMAND ${PIP_CMG} -qP ${CCM_OPTS} -o ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_ccm -I${PIP_INCLUDES} ${CMAKE_CURRENT_SOURCE_DIR}/${CCM_SRC})
|
#execute_process(OUTPUT_VARIABLE CMG_OUT COMMAND ${PIP_CMG} -qP ${CCM_OPTS} -o ${CMAKE_CURRENT_BINARY_DIR}/$ccm_{PROJECT_NAME} -I${PIP_INCLUDES} ${CMAKE_CURRENT_SOURCE_DIR}/${CCM_SRC})
|
||||||
#message(STATUS "Creating code model done, to use it include \"${PROJECT_NAME}_ccm.h\"")
|
#message(STATUS "Creating code model done, to use it include \"ccm_${PROJECT_NAME}.h\"")
|
||||||
#message(STATUS ${CMG_OUT})
|
#message(STATUS ${CMG_OUT})
|
||||||
#message(STATUS ${CACHE_CMG_FILES})
|
#message(STATUS ${CACHE_CMG_FILES})
|
||||||
endmacro (pip_code_model)
|
endmacro (pip_code_model)
|
||||||
|
|||||||
@@ -27,5 +27,6 @@
|
|||||||
#include "picrc.h"
|
#include "picrc.h"
|
||||||
#include "picrypt.h"
|
#include "picrypt.h"
|
||||||
#include "pifixedpoint.h"
|
#include "pifixedpoint.h"
|
||||||
|
#include "piquaternion.h"
|
||||||
|
|
||||||
#endif // PIMATHMODULE_H
|
#endif // PIMATHMODULE_H
|
||||||
|
|||||||
209
src/math/piquaternion.cpp
Normal file
209
src/math/piquaternion.cpp
Normal file
@@ -0,0 +1,209 @@
|
|||||||
|
#include "piquaternion.h"
|
||||||
|
|
||||||
|
|
||||||
|
PIQuaternion::PIQuaternion(const PIMathVectorT3d & u, double a) {
|
||||||
|
q[0] = a;
|
||||||
|
q[1] = u[0];
|
||||||
|
q[2] = u[1];
|
||||||
|
q[3] = u[2];
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
PIMathVectorT3d PIQuaternion::eyler() const {
|
||||||
|
PIMathMatrixT33d rmat = rotationMatrix();
|
||||||
|
double angle_y = asin(rmat[0][2]), angle_x, angle_z;
|
||||||
|
double c = cos(angle_y);
|
||||||
|
if (fabs(c) < 1.E-5) {
|
||||||
|
angle_x = 0;
|
||||||
|
angle_z = atan2(rmat[1][0], rmat[1][1]);
|
||||||
|
} else {
|
||||||
|
angle_x = -atan2(-rmat[1][2] / c, rmat[2][2] / c);
|
||||||
|
angle_z = atan2(-rmat[0][1] / c, rmat[0][0] / c);
|
||||||
|
}
|
||||||
|
if (angle_z < 0) angle_z = 2*M_PI + angle_z;
|
||||||
|
return PIMathVectorT3d(angle_x,angle_y,angle_z);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
PIMathMatrixT33d PIQuaternion::rotationMatrix() const {
|
||||||
|
PIMathMatrixT33d ret;
|
||||||
|
double xx = q[1] * q[1];
|
||||||
|
double xy = q[1] * q[2];
|
||||||
|
double xz = q[1] * q[3];
|
||||||
|
double xw = q[1] * q[0];
|
||||||
|
double yy = q[2] * q[2];
|
||||||
|
double yz = q[2] * q[3];
|
||||||
|
double yw = q[2] * q[0];
|
||||||
|
double zz = q[3] * q[3];
|
||||||
|
double zw = q[3] * q[0];
|
||||||
|
ret[0][0] = 1. - 2. * (yy + zz);
|
||||||
|
ret[0][1] = 2. * (xy - zw);
|
||||||
|
ret[0][2] = 2. * (xz + yw);
|
||||||
|
ret[1][0] = 2. * (xy + zw);
|
||||||
|
ret[1][1] = 1. - 2. * (xx + zz);
|
||||||
|
ret[1][2] = 2. * (yz - xw);
|
||||||
|
ret[2][0] = 2. * (xz - yw);
|
||||||
|
ret[2][1] = 2. * (yz + xw);
|
||||||
|
ret[2][2] = 1. - 2. * (xx + yy);
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void PIQuaternion::axis(PIMathVectorT3d * ret) const {
|
||||||
|
if (!ret) return;
|
||||||
|
ret[0] = vector();
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
PIQuaternion PIQuaternion::rotated(const PIMathVectorT3d & u, double a) const {
|
||||||
|
PIQuaternion ret(*this);
|
||||||
|
ret.rotate(u, a);
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void PIQuaternion::rotate(const PIMathVectorT3d & u, double a) {
|
||||||
|
PIQuaternion v(u * sin(a / 2.), cos(a / 2.));
|
||||||
|
/*PIMathMatrixT44d r = v.makeMatrix() * makeMatrix() * v.inverted().makeMatrix();
|
||||||
|
q[0] = r[0][0];
|
||||||
|
q[1] = r[1][0];
|
||||||
|
q[2] = r[2][0];
|
||||||
|
q[3] = r[3][0];*/
|
||||||
|
*this = (v * (*this) * v.conjugate());
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void PIQuaternion::normalize() {
|
||||||
|
double d = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
|
||||||
|
if (d != 0.)
|
||||||
|
for (int i = 0; i < 4; ++i)
|
||||||
|
q[i] /= d;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
PIMathMatrixT44d PIQuaternion::makeMatrix() const {
|
||||||
|
PIMathMatrixT44d ret;
|
||||||
|
ret[0][0] = q[0]; ret[0][1] = -q[1]; ret[0][2] = -q[2]; ret[0][3] = -q[3];
|
||||||
|
ret[1][0] = q[1]; ret[1][1] = q[0]; ret[1][2] = -q[3]; ret[1][3] = q[2];
|
||||||
|
ret[2][0] = q[2]; ret[2][1] = q[3]; ret[2][2] = q[0]; ret[2][3] = -q[1];
|
||||||
|
ret[3][0] = q[3]; ret[3][1] = -q[2]; ret[3][2] = q[1]; ret[3][3] = q[0];
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
PIQuaternion PIQuaternion::fromEyler(double ax, double ay, double az) {
|
||||||
|
PIQuaternion q_heading;
|
||||||
|
PIQuaternion q_pinch;
|
||||||
|
PIQuaternion q_bank;
|
||||||
|
PIQuaternion q_tmp;
|
||||||
|
q_heading.q[0] = 0;
|
||||||
|
q_heading.q[1] = 0;
|
||||||
|
q_heading.q[2] = sin(ax / 2);
|
||||||
|
q_heading.q[3] = -cos(ax / 2);
|
||||||
|
q_pinch.q[0] = 0;
|
||||||
|
q_pinch.q[1] = sin(ay / 2);
|
||||||
|
q_pinch.q[2] = 0;
|
||||||
|
q_pinch.q[3] = -cos(ay / 2);
|
||||||
|
az = M_PI - az;
|
||||||
|
q_bank.q[0] = sin(az / 2);
|
||||||
|
q_bank.q[1] = 0;
|
||||||
|
q_bank.q[2] = 0;
|
||||||
|
q_bank.q[3] = cos(az / 2);
|
||||||
|
q_tmp = q_heading * q_pinch;
|
||||||
|
q_tmp = q_tmp * q_bank;
|
||||||
|
q_tmp.normalize();
|
||||||
|
return q_tmp;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
PIQuaternion PIQuaternion::fromAngles(double ax, double ay, double az) {
|
||||||
|
double c1 = cos(ay/2);
|
||||||
|
double s1 = sin(ay/2);
|
||||||
|
double c2 = cos(az/2);
|
||||||
|
double s2 = sin(az/2);
|
||||||
|
double c3 = cos(ax/2);
|
||||||
|
double s3 = sin(ax/2);
|
||||||
|
double c1c2 = c1*c2;
|
||||||
|
double s1s2 = s1*s2;
|
||||||
|
double a = c1c2*c3 - s1s2*s3;
|
||||||
|
double x = c1c2*s3 + s1s2*c3;
|
||||||
|
double y = s1*c2*c3 + c1*s2*s3;
|
||||||
|
double z = c1*s2*c3 - s1*c2*s3;
|
||||||
|
PIQuaternion res;
|
||||||
|
res.q[0] = a;
|
||||||
|
res.q[1] = x;
|
||||||
|
res.q[2] = y;
|
||||||
|
res.q[3] = z;
|
||||||
|
return res;
|
||||||
|
return PIQuaternion(PIMathVectorT3d(x,y,z),a);
|
||||||
|
}
|
||||||
|
|
||||||
|
PIQuaternion PIQuaternion::fromAngles2(double ax, double ay, double az) {
|
||||||
|
double om = PIMathVectorT3d(ax,ay,az).length();
|
||||||
|
// az = M_PI - az;
|
||||||
|
if (om == 0.) return PIQuaternion(PIMathVectorT3d(), 1.);
|
||||||
|
PIQuaternion res;
|
||||||
|
res.q[0] = cos(om/2);
|
||||||
|
res.q[1] = ax/om * sin(om/2);
|
||||||
|
res.q[2] = ay/om * sin(om/2);
|
||||||
|
res.q[3] = az/om * sin(om/2);
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
PIQuaternion PIQuaternion::fromRotationMatrix(const PIMathMatrixT33d & m) {
|
||||||
|
PIQuaternion ret;
|
||||||
|
float tr = m[0][0] + m[1][1] + m[2][2];
|
||||||
|
if (tr > 0.0f) {
|
||||||
|
ret.q[0] = tr + 1.0f;
|
||||||
|
ret.q[1] = m[2][1] - m[1][2];
|
||||||
|
ret.q[2] = m[0][2] - m[2][0];
|
||||||
|
ret.q[3] = m[1][0] - m[0][1];
|
||||||
|
} else {
|
||||||
|
if ((m[0][0] > m[1][1]) && (m[0][0] > m[2][2])) {
|
||||||
|
ret.q[0] = m[2][1] - m[1][2];
|
||||||
|
ret.q[1] = 1.0f + m[0][0] - m[1][1] - m[2][2];
|
||||||
|
ret.q[2] = m[0][1] + m[1][0];
|
||||||
|
ret.q[3] = m[0][2] + m[2][0];
|
||||||
|
} else {
|
||||||
|
if (m[1][1] > m[2][2]) {
|
||||||
|
ret.q[0] = m[0][2] - m[2][0];
|
||||||
|
ret.q[1] = m[0][1] + m[1][0];
|
||||||
|
ret.q[2] = 1.0f + m[1][1] - m[0][0] - m[2][2];
|
||||||
|
ret.q[3] = m[1][2] + m[2][1];
|
||||||
|
} else {
|
||||||
|
ret.q[0] = m[1][0] - m[0][1];
|
||||||
|
ret.q[1] = m[0][2] + m[2][0];
|
||||||
|
ret.q[2] = m[1][2] + m[2][1];
|
||||||
|
ret.q[3] = 1.0f + m[2][2] - m[0][0] - m[1][1];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
ret.normalize();
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
PIQuaternion operator*(const PIQuaternion & q0, const PIQuaternion & q1) {
|
||||||
|
PIMathVectorT3d v0(q0.vector()), v1(q1.vector());
|
||||||
|
double r0 = q0.q[0] * q1.q[0] - (v0^v1);
|
||||||
|
PIMathVectorT3d qv = v1*q0.q[0] + v0*q1.q[0] + v0*v1;
|
||||||
|
PIQuaternion ret;
|
||||||
|
ret.q[0] = r0;
|
||||||
|
ret.q[1] = qv[0];
|
||||||
|
ret.q[2] = qv[1];
|
||||||
|
ret.q[3] = qv[2];
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
PIQuaternion operator *(const double &a, const PIQuaternion &q1) {
|
||||||
|
PIQuaternion ret(q1);
|
||||||
|
ret.q[0] *= a;
|
||||||
|
ret.q[1] *= a;
|
||||||
|
ret.q[2] *= a;
|
||||||
|
ret.q[3] *= a;
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
67
src/math/piquaternion.h
Normal file
67
src/math/piquaternion.h
Normal file
@@ -0,0 +1,67 @@
|
|||||||
|
/*! \file piquaternion.h
|
||||||
|
* \brief Class for quaternions
|
||||||
|
*/
|
||||||
|
/*
|
||||||
|
PIP - Platform Independent Primitives
|
||||||
|
Class for quaternions
|
||||||
|
Copyright (C) 2016 Ivan Pelipenko peri4ko@yandex.ru, Andrey Bychkov work.a.b@yandex.ru
|
||||||
|
|
||||||
|
This program is free software: you can redistribute it and/or modify
|
||||||
|
it under the terms of the GNU General Public License as published by
|
||||||
|
the Free Software Foundation, either version 3 of the License, or
|
||||||
|
(at your option) any later version.
|
||||||
|
|
||||||
|
This program is distributed in the hope that it will be useful,
|
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
GNU General Public License for more details.
|
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License
|
||||||
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef PIQUATERNION_H
|
||||||
|
#define PIQUATERNION_H
|
||||||
|
|
||||||
|
#include "pimathmatrix.h"
|
||||||
|
|
||||||
|
class PIQuaternion
|
||||||
|
{
|
||||||
|
friend PIQuaternion operator*(const PIQuaternion & q0, const PIQuaternion & q1);
|
||||||
|
friend PIQuaternion operator*(const double & a, const PIQuaternion & q);
|
||||||
|
public:
|
||||||
|
PIQuaternion(const PIMathVectorT3d & u = PIMathVectorT3d(), double a = 0.);
|
||||||
|
|
||||||
|
PIQuaternion conjugate() const {return PIQuaternion(-vector(), scalar());}
|
||||||
|
PIQuaternion rotated(const PIMathVectorT3d & u, double a) const;
|
||||||
|
void rotate(const PIMathVectorT3d & u, double a);
|
||||||
|
void normalize();
|
||||||
|
|
||||||
|
double & scalar() {return q[0];}
|
||||||
|
double scalar() const {return q[0];}
|
||||||
|
PIMathVectorT3d vector() const {return PIMathVectorT3d(q[1], q[2], q[3]);}
|
||||||
|
|
||||||
|
PIMathVectorT3d eyler() const;
|
||||||
|
PIMathMatrixT33d rotationMatrix() const;
|
||||||
|
void axis(PIMathVectorT3d*ret) const;
|
||||||
|
|
||||||
|
static PIQuaternion fromEyler(double ax, double ay, double az);
|
||||||
|
static PIQuaternion fromRotationMatrix(const PIMathMatrixT33d & m);
|
||||||
|
static PIQuaternion fromAngles(double ax, double ay, double az);
|
||||||
|
static PIQuaternion fromAngles2(double ax, double ay, double az);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
double q[4];
|
||||||
|
PIMathMatrixT44d makeMatrix() const;
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
PIQuaternion operator *(const double & a, const PIQuaternion & q);
|
||||||
|
//PIQuaternion operator *(const PIQuaternion & q, const double & a) {return a*q;}
|
||||||
|
PIQuaternion operator *(const PIQuaternion & q0, const PIQuaternion & q1);
|
||||||
|
inline PIQuaternion operator +(const PIQuaternion & q0, const PIQuaternion & q1) {return PIQuaternion(q0.vector() + q1.vector(), q0.scalar() + q1.scalar());}
|
||||||
|
inline PIQuaternion operator -(const PIQuaternion & q0, const PIQuaternion & q1) {return PIQuaternion(q0.vector() - q1.vector(), q0.scalar() - q1.scalar());}
|
||||||
|
inline PIQuaternion operator -(const PIQuaternion & q0) {return PIQuaternion(-q0.vector(), -q0.scalar());}
|
||||||
|
|
||||||
|
#endif // PIQUATERNION_H
|
||||||
Reference in New Issue
Block a user