Commit 6efc8ec1 authored by Dmitri Fix's avatar Dmitri Fix
Browse files

Bug-fixes and corrections:

 - re-simulation
 - variation
 - copyright headers

Updates:
 - update PCM installation guide
 - remove duplicate sources
 - drop deprecated basic use-case
 - add synthetic PCM simulation result sample
parent 9e1daeda
......@@ -4,3 +4,4 @@ Thumbs.db
*.orig
*.rej
*.user
*.bak
# /*********************************************************************
# * Copyright (c) 2016 ITK Engineering GmbH
# *
# * This program and the accompanying materials are made
# * available under the terms of the Eclipse Public License 2.0
# * which is available at https://www.eclipse.org/legal/epl-2.0/
# *
# * SPDX-License-Identifier: EPL-2.0
# **********************************************************************/
TEMPLATE = subdirs
SUBDIRS = \
openPASS/OpenPass_Basic.pro
......@@ -80,16 +80,16 @@ bool PCM_Data::AddPCM_Agent(int id, double xPos, double yPos, double yawAngle, d
PCM_Line *line = new PCM_Line(0);
std::vector<Common::Vector2d> pointVector;
pointVector.push_back(Common::Vector2d(width / 2, height / 2));
pointVector.push_back(Common::Vector2d(-width / 2, height / 2));
pointVector.push_back(Common::Vector2d(-width / 2, -height / 2));
pointVector.push_back(Common::Vector2d(width / 2, -height / 2));
pointVector.push_back(Common::Vector2d(width / 2, height / 2));// in vehicle coordinates
pointVector.push_back(Common::Vector2d(-width / 2, height / 2));// in vehicle coordinates
pointVector.push_back(Common::Vector2d(-width / 2, -height / 2));// in vehicle coordinates
pointVector.push_back(Common::Vector2d(width / 2, -height / 2));// in vehicle coordinates
int count = 0;
for (Common::Vector2d point : pointVector)
{
point.Rotate(yawAngle);
point.Translate(xPos, yPos);
point.Rotate(yawAngle);//from vehicle to world coordinates
point.Translate(xPos, yPos);//from vehicle to world coordinates
line->AddPCM_Point(new PCM_Point(count++, point.x, point.y));
}
......
......@@ -80,12 +80,12 @@ QString PCM_Trajectory::VecIntToCsv(const std::vector<int> *inVec) const
{
QString outString = "";
// for (uint index = 0; index < inVec->size() - 1; index++)
// {
// outString += (QString::number(inVec->at(index), 'g', 10) + ", ");
// }
// for (uint index = 0; index < inVec->size() - 1; index++)
// {
// outString += (QString::number(inVec->at(index), 'g', 10) + ", ");
// }
// outString += QString::number(inVec->at(inVec->size() - 1), 'g', 10);
// outString += QString::number(inVec->at(inVec->size() - 1), 'g', 10);
......@@ -116,22 +116,41 @@ QString PCM_Trajectory::VecDoubleToCsv(const std::vector<double> *inVec) const
outString += QString::number(inVec->at(inVec->size() - 1), 'g', 7);
return outString;
}
// if (!inVec->empty())
// {
// std::ostringstream oss;
PCM_Trajectory* PCM_Trajectory::Copy(const PCM_Trajectory* trajectory)
{
// // Convert all but the last element to avoid a trailing ","
// std::copy(inVec->begin(), inVec->end()-1, std::ostream_iterator<double>(oss, ", "));
size_t sizeOfTrajectory = trajectory->GetTimeVec()->size();
// // Now add the last element with no delimiter
// oss << inVec->back();
// outString = QString::fromStdString(oss.str());
// }
std::vector<int> *timeVec1 = new std::vector<int>;
std::vector<double> *xPosVec1 = new std::vector<double>;
std::vector<double> *yPosVec1 = new std::vector<double>;
std::vector<double> *uVelVec1 = new std::vector<double>;
std::vector<double> *vVelVec1 = new std::vector<double>;
std::vector<double> *psiVec1 = new std::vector<double>;
for (size_t it = 0; it<sizeOfTrajectory; ++it)
{
timeVec1->push_back(trajectory->GetTimeVec()->at(it));
xPosVec1->push_back(trajectory->GetXPosVec()->at(it));
yPosVec1->push_back(trajectory->GetYPosVec()->at(it));
uVelVec1->push_back(trajectory->GetUVelVec()->at(it));
vVelVec1->push_back(trajectory->GetVVelVec()->at(it));
psiVec1->push_back(trajectory->GetPsiVec()->at(it));
}
return new PCM_Trajectory(timeVec1,
xPosVec1,
yPosVec1,
uVelVec1,
vVelVec1,
psiVec1);
return outString;
}
void PCM_Trajectory::Clear()
{
if (timeVec != nullptr)
......
......@@ -28,7 +28,7 @@ class PCM_Trajectory
public:
PCM_Trajectory() = default;
//-----------------------------------------------------------------------------
//! Constructor
//! Constructor from vectors
//!
//! @param[in] timeVec The vector of the time of the trajectory points
//! @param[in] xPosVec The vector of the x-coordinates of the trajectory points
......@@ -108,18 +108,21 @@ public:
//! Clear all trajectory data.
void Clear();
//! Static copy "constructor" from trajectory
//!
//! @param[in] trajectory Trajectory to be copied
//-----------------------------------------------------------------------------
static PCM_Trajectory* Copy(const PCM_Trajectory*);
private:
const std::vector<int> *timeVec = nullptr; //!< vector of the time of the trajectory points
const std::vector<double> *xPosVec =
nullptr; //!< vector of the x-coordinates of the trajectory points
const std::vector<double> *yPosVec =
nullptr; //!< vector of the y-coordinates of the trajectory points
const std::vector<double> *uVelVec =
nullptr; //!< vector of the velocities of the trajectory points (forward)
const std::vector<double> *vVelVec =
nullptr; //!< vector of the velocities of the trajectory points (sideward)
const std::vector<double> *xPosVec = nullptr; //!< vector of the x-coordinates of the trajectory points
const std::vector<double> *yPosVec = nullptr; //!< vector of the y-coordinates of the trajectory points
const std::vector<double> *uVelVec = nullptr; //!< vector of the velocities of the trajectory points (forward)
const std::vector<double> *vVelVec = nullptr; //!< vector of the velocities of the trajectory points (sideward)
const std::vector<double> *psiVec = nullptr; //!< vector of the yaw angles of the trajectory points
};
#endif // PCM_TRAJECTORY_H
......@@ -21,9 +21,9 @@ enum class PCMdefaultPrio
{
Algorithm_Selector = 100, //100
Algorithm_Trajectory = 150, //105
Dynamics_Collision = 1, //0
Dynamics_Collision = 0, //0
Dynamics = 3, //1
EgoSensor = 203, //203
Init_Agent = 0, //0
Init_Agent = 500, //500
Sensor_Collision = 201 //201
};
/* const.c
*
* Globally declared constants
*
*
*
* SYNOPSIS:
*
* extern double nameofconstant;
*
*
*
*
* DESCRIPTION:
*
* This file contains a number of mathematical constants and
* also some needed size parameters of the computer arithmetic.
* The values are supplied as arrays of hexadecimal integers
* for IEEE arithmetic; arrays of octal constants for DEC
* arithmetic; and in a normal decimal scientific notation for
* other machines. The particular notation used is determined
* by a symbol (DEC, IBMPC, or UNK) defined in the include file
* mconf.h.
*
* The default size parameters are as follows.
*
* For DEC and UNK modes:
* MACHEP = 1.38777878078144567553E-17 2**-56
* MAXLOG = 8.8029691931113054295988E1 log(2**127)
* MINLOG = -8.872283911167299960540E1 log(2**-128)
* MAXNUM = 1.701411834604692317316873e38 2**127
*
* For IEEE arithmetic (IBMPC):
* MACHEP = 1.11022302462515654042E-16 2**-53
* MAXLOG = 7.09782712893383996843E2 log(2**1024)
* MINLOG = -7.08396418532264106224E2 log(2**-1022)
* MAXNUM = 1.7976931348623158E308 2**1024
*
* The global symbols for mathematical constants are
* PI = 3.14159265358979323846 pi
* PIO2 = 1.57079632679489661923 pi/2
* PIO4 = 7.85398163397448309616E-1 pi/4
* SQRT2 = 1.41421356237309504880 sqrt(2)
* SQRTH = 7.07106781186547524401E-1 sqrt(2)/2
* LOG2E = 1.4426950408889634073599 1/log(2)
* SQ2OPI = 7.9788456080286535587989E-1 sqrt( 2/pi )
* LOGE2 = 6.93147180559945309417E-1 log(2)
* LOGSQ2 = 3.46573590279972654709E-1 log(2)/2
* THPIO4 = 2.35619449019234492885 3*pi/4
* TWOOPI = 6.36619772367581343075535E-1 2/pi
*
* These lists are subject to change.
*/
/* const.c */
/*
Cephes Math Library Release 2.3: March, 1995
Copyright 1984, 1995 by Stephen L. Moshier
*/
#include "mconf.h"
#ifdef UNK
#if 1
double MACHEP = 1.11022302462515654042E-16; /* 2**-53 */
#else
double MACHEP = 1.38777878078144567553E-17; /* 2**-56 */
#endif
double UFLOWTHRESH = 2.22507385850720138309E-308; /* 2**-1022 */
#ifdef DENORMAL
double MAXLOG = 7.09782712893383996732E2; /* log(MAXNUM) */
/* double MINLOG = -7.44440071921381262314E2; */ /* log(2**-1074) */
double MINLOG = -7.451332191019412076235E2; /* log(2**-1075) */
#else
double MAXLOG = 7.08396418532264106224E2; /* log 2**1022 */
double MINLOG = -7.08396418532264106224E2; /* log 2**-1022 */
#endif
double MAXNUM = 1.79769313486231570815E308; /* 2**1024*(1-MACHEP) */
double PI = 3.14159265358979323846; /* pi */
double PIO2 = 1.57079632679489661923; /* pi/2 */
double PIO4 = 7.85398163397448309616E-1; /* pi/4 */
double SQRT2 = 1.41421356237309504880; /* sqrt(2) */
double SQRTH = 7.07106781186547524401E-1; /* sqrt(2)/2 */
double LOG2E = 1.4426950408889634073599; /* 1/log(2) */
double SQ2OPI = 7.9788456080286535587989E-1; /* sqrt( 2/pi ) */
double LOGE2 = 6.93147180559945309417E-1; /* log(2) */
double LOGSQ2 = 3.46573590279972654709E-1; /* log(2)/2 */
double THPIO4 = 2.35619449019234492885; /* 3*pi/4 */
double TWOOPI = 6.36619772367581343075535E-1; /* 2/pi */
#ifdef INFINITIES
double INFINITY = 1.0/0.0; /* 99e999; */
#else
double INFINITY = 1.79769313486231570815E308; /* 2**1024*(1-MACHEP) */
#endif
#ifdef NANS
double NAN = 1.0/0.0 - 1.0/0.0;
#else
double NAN = 0.0;
#endif
#ifdef MINUSZERO
double NEGZERO = -0.0;
#else
double NEGZERO = 0.0;
#endif
#endif
#ifdef IBMPC
/* 2**-53 = 1.11022302462515654042E-16 */
unsigned short MACHEP[4] = {0x0000,0x0000,0x0000,0x3ca0};
unsigned short UFLOWTHRESH[4] = {0x0000,0x0000,0x0000,0x0010};
#ifdef DENORMAL
/* log(MAXNUM) = 7.09782712893383996732224E2 */
unsigned short MAXLOG[4] = {0x39ef,0xfefa,0x2e42,0x4086};
/* log(2**-1074) = - -7.44440071921381262314E2 */
/*unsigned short MINLOG[4] = {0x71c3,0x446d,0x4385,0xc087};*/
unsigned short MINLOG[4] = {0x3052,0xd52d,0x4910,0xc087};
#else
/* log(2**1022) = 7.08396418532264106224E2 */
unsigned short MAXLOG[4] = {0xbcd2,0xdd7a,0x232b,0x4086};
/* log(2**-1022) = - 7.08396418532264106224E2 */
unsigned short MINLOG[4] = {0xbcd2,0xdd7a,0x232b,0xc086};
#endif
/* 2**1024*(1-MACHEP) = 1.7976931348623158E308 */
unsigned short MAXNUM[4] = {0xffff,0xffff,0xffff,0x7fef};
unsigned short PI[4] = {0x2d18,0x5444,0x21fb,0x4009};
unsigned short PIO2[4] = {0x2d18,0x5444,0x21fb,0x3ff9};
unsigned short PIO4[4] = {0x2d18,0x5444,0x21fb,0x3fe9};
unsigned short SQRT2[4] = {0x3bcd,0x667f,0xa09e,0x3ff6};
unsigned short SQRTH[4] = {0x3bcd,0x667f,0xa09e,0x3fe6};
unsigned short LOG2E[4] = {0x82fe,0x652b,0x1547,0x3ff7};
unsigned short SQ2OPI[4] = {0x3651,0x33d4,0x8845,0x3fe9};
unsigned short LOGE2[4] = {0x39ef,0xfefa,0x2e42,0x3fe6};
unsigned short LOGSQ2[4] = {0x39ef,0xfefa,0x2e42,0x3fd6};
unsigned short THPIO4[4] = {0x21d2,0x7f33,0xd97c,0x4002};
unsigned short TWOOPI[4] = {0xc883,0x6dc9,0x5f30,0x3fe4};
#ifdef INFINITIES
unsigned short INFINITY[4] = {0x0000,0x0000,0x0000,0x7ff0};
#else
unsigned short INFINITY[4] = {0xffff,0xffff,0xffff,0x7fef};
#endif
#ifdef NANS
unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x7ffc};
#else
unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x0000};
#endif
#ifdef MINUSZERO
unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x8000};
#else
unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x0000};
#endif
#endif
#ifdef MIEEE
/* 2**-53 = 1.11022302462515654042E-16 */
unsigned short MACHEP[4] = {0x3ca0,0x0000,0x0000,0x0000};
unsigned short UFLOWTHRESH[4] = {0x0010,0x0000,0x0000,0x0000};
#ifdef DENORMAL
/* log(2**1024) = 7.09782712893383996843E2 */
unsigned short MAXLOG[4] = {0x4086,0x2e42,0xfefa,0x39ef};
/* log(2**-1074) = - -7.44440071921381262314E2 */
/* unsigned short MINLOG[4] = {0xc087,0x4385,0x446d,0x71c3}; */
unsigned short MINLOG[4] = {0xc087,0x4910,0xd52d,0x3052};
#else
/* log(2**1022) = 7.08396418532264106224E2 */
unsigned short MAXLOG[4] = {0x4086,0x232b,0xdd7a,0xbcd2};
/* log(2**-1022) = - 7.08396418532264106224E2 */
unsigned short MINLOG[4] = {0xc086,0x232b,0xdd7a,0xbcd2};
#endif
/* 2**1024*(1-MACHEP) = 1.7976931348623158E308 */
unsigned short MAXNUM[4] = {0x7fef,0xffff,0xffff,0xffff};
unsigned short PI[4] = {0x4009,0x21fb,0x5444,0x2d18};
unsigned short PIO2[4] = {0x3ff9,0x21fb,0x5444,0x2d18};
unsigned short PIO4[4] = {0x3fe9,0x21fb,0x5444,0x2d18};
unsigned short SQRT2[4] = {0x3ff6,0xa09e,0x667f,0x3bcd};
unsigned short SQRTH[4] = {0x3fe6,0xa09e,0x667f,0x3bcd};
unsigned short LOG2E[4] = {0x3ff7,0x1547,0x652b,0x82fe};
unsigned short SQ2OPI[4] = {0x3fe9,0x8845,0x33d4,0x3651};
unsigned short LOGE2[4] = {0x3fe6,0x2e42,0xfefa,0x39ef};
unsigned short LOGSQ2[4] = {0x3fd6,0x2e42,0xfefa,0x39ef};
unsigned short THPIO4[4] = {0x4002,0xd97c,0x7f33,0x21d2};
unsigned short TWOOPI[4] = {0x3fe4,0x5f30,0x6dc9,0xc883};
#ifdef INFINITIES
unsigned short INFINITY[4] = {0x7ff0,0x0000,0x0000,0x0000};
#else
unsigned short INFINITY[4] = {0x7fef,0xffff,0xffff,0xffff};
#endif
#ifdef NANS
unsigned short NAN[4] = {0x7ff8,0x0000,0x0000,0x0000};
#else
unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x0000};
#endif
#ifdef MINUSZERO
unsigned short NEGZERO[4] = {0x8000,0x0000,0x0000,0x0000};
#else
unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x0000};
#endif
#endif
#ifdef DEC
/* 2**-56 = 1.38777878078144567553E-17 */
unsigned short MACHEP[4] = {0022200,0000000,0000000,0000000};
unsigned short UFLOWTHRESH[4] = {0x0080,0x0000,0x0000,0x0000};
/* log 2**127 = 88.029691931113054295988 */
unsigned short MAXLOG[4] = {041660,007463,0143742,025733,};
/* log 2**-128 = -88.72283911167299960540 */
unsigned short MINLOG[4] = {0141661,071027,0173721,0147572,};
/* 2**127 = 1.701411834604692317316873e38 */
unsigned short MAXNUM[4] = {077777,0177777,0177777,0177777,};
unsigned short PI[4] = {040511,007732,0121041,064302,};
unsigned short PIO2[4] = {040311,007732,0121041,064302,};
unsigned short PIO4[4] = {040111,007732,0121041,064302,};
unsigned short SQRT2[4] = {040265,002363,031771,0157145,};
unsigned short SQRTH[4] = {040065,002363,031771,0157144,};
unsigned short LOG2E[4] = {040270,0125073,024534,013761,};
unsigned short SQ2OPI[4] = {040114,041051,0117241,0131204,};
unsigned short LOGE2[4] = {040061,071027,0173721,0147572,};
unsigned short LOGSQ2[4] = {037661,071027,0173721,0147572,};
unsigned short THPIO4[4] = {040426,0145743,0174631,007222,};
unsigned short TWOOPI[4] = {040042,0174603,067116,042025,};
/* Approximate infinity by MAXNUM. */
unsigned short INFINITY[4] = {077777,0177777,0177777,0177777,};
unsigned short NAN[4] = {0000000,0000000,0000000,0000000};
#ifdef MINUSZERO
unsigned short NEGZERO[4] = {0000000,0000000,0000000,0100000};
#else
unsigned short NEGZERO[4] = {0000000,0000000,0000000,0000000};
#endif
#endif
#ifndef UNK
extern unsigned short MACHEP[];
extern unsigned short UFLOWTHRESH[];
extern unsigned short MAXLOG[];
extern unsigned short UNDLOG[];
extern unsigned short MINLOG[];
extern unsigned short MAXNUM[];
extern unsigned short PI[];
extern unsigned short PIO2[];
extern unsigned short PIO4[];
extern unsigned short SQRT2[];
extern unsigned short SQRTH[];
extern unsigned short LOG2E[];
extern unsigned short SQ2OPI[];
extern unsigned short LOGE2[];
extern unsigned short LOGSQ2[];
extern unsigned short THPIO4[];
extern unsigned short TWOOPI[];
extern unsigned short INFINITY[];
extern unsigned short NAN[];
extern unsigned short NEGZERO[];
#endif
/* fresnl.c
*
* Fresnel integral
*
*
*
* SYNOPSIS:
*
* double x, S, C;
* void fresnl();
*
* fresnl( x, _&S, _&C );
*
*
* DESCRIPTION:
*
* Evaluates the Fresnel integrals
*
* x
* -
* | |
* C(x) = | cos(pi/2 t**2) dt,
* | |
* -
* 0
*
* x
* -
* | |
* S(x) = | sin(pi/2 t**2) dt.
* | |
* -
* 0
*
*
* The integrals are evaluated by a power series for x < 1.
* For x >= 1 auxiliary functions f(x) and g(x) are employed
* such that
*
* C(x) = 0.5 + f(x) sin( pi/2 x**2 ) - g(x) cos( pi/2 x**2 )
* S(x) = 0.5 - f(x) cos( pi/2 x**2 ) - g(x) sin( pi/2 x**2 )
*
*
*
* ACCURACY:
*
* Relative error.
*
* Arithmetic function domain # trials peak rms
* IEEE S(x) 0, 10 10000 2.0e-15 3.2e-16
* IEEE C(x) 0, 10 10000 1.8e-15 3.3e-16
* DEC S(x) 0, 10 6000 2.2e-16 3.9e-17
* DEC C(x) 0, 10 5000 2.3e-16 3.9e-17
*/
/*
Cephes Math Library Release 2.8: June, 2000
Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier
*/
#include "mconf.h"
/* S(x) for small x */
#ifdef UNK
static double sn[6] = {
-2.99181919401019853726E3,
7.08840045257738576863E5,
-6.29741486205862506537E7,
2.54890880573376359104E9,
-4.42979518059697779103E10,
3.18016297876567817986E11,
};
static double sd[6] = {
/* 1.00000000000000000000E0,*/
2.81376268889994315696E2,
4.55847810806532581675E4,
5.17343888770096400730E6,
4.19320245898111231129E8,
2.24411795645340920940E10,
6.07366389490084639049E11,
};
#endif
#ifdef DEC
static unsigned short sn[24] = {
0143072,0176433,0065455,0127034,
0045055,0007200,0134540,0026661,
0146560,0035061,0023667,0127545,
0050027,0166503,0002673,0153756,
0151045,0002721,0121737,0102066,
0051624,0013177,0033451,0021271,
};
static unsigned short sd[24] = {
/*0040200,0000000,0000000,0000000,*/
0042214,0130051,0112070,0101617,
0044062,0010307,0172346,0152510,
0045635,0160575,0143200,0136642,
0047307,0171215,0127457,0052361,
0050647,0031447,0032621,0013510,
0052015,0064733,0117362,0012653,
};
#endif
#ifdef IBMPC
static unsigned short sn[24] = {
0xb5c3,0x6d65,0x5fa3,0xc0a7,
0x05b6,0x172c,0xa1d0,0x4125,
0xf5ed,0x24f6,0x0746,0xc18e,
0x7afe,0x60b7,0xfda8,0x41e2,
0xf087,0x347b,0xa0ba,0xc224,
0x2457,0xe6e5,0x82cf,0x4252,
};
static unsigned short sd[24] = {
/*0x0000,0x0000,0x0000,0x3ff0,*/
0x1072,0x3287,0x9605,0x4071,
0xdaa9,0xfe9c,0x4218,0x40e6,
0x17b4,0xb8d0,0xbc2f,0x4153,
0xea9e,0xb5e5,0xfe51,0x41b8,
0x22e9,0xe6b2,0xe664,0x4214,
0x42b5,0x73de,0xad3b,0x4261,
};
#endif
#ifdef MIEEE
static unsigned short sn[24] = {
0xc0a7,0x5fa3,0x6d65,0xb5c3,
0x4125,0xa1d0,0x172c,0x05b6,
0xc18e,0x0746,0x24f6,0xf5ed,
0x41e2,0xfda8,0x60b7,0x7afe,
0xc224,0xa0ba,0x347b,0xf087,
0x4252,0x82cf,0xe6e5,0x2457,
};
static unsigned short sd[24] = {
/*0x3ff0,0x0000,0x0000,0x0000,*/
0x4071,0x9605,0x3287,0x1072,
0x40e6,0x4218,0xfe9c,0xdaa9,
0x4153,0xbc2f,0xb8d0,0x17b4,
0x41b8,0xfe51,0xb5e5,0xea9e,
0x4214,0xe664,0xe6b2,0x22e9,
0x4261,0xad3b,0x73de,0x42b5,
};
#endif
/* C(x) for small x */
#ifdef UNK
static double cn[6] = {
-4.98843114573573548651E-8,
9.50428062829859605134E-6,
-6.45191435683965050962E-4,
1.88843319396703850064E-2,
-2.05525900955013891793E-1,
9.99999999999999998822E-1,
};
static double cd[7] = {