/**
* \fn Numerique Numerique::operatorAND(const Numerique& n)
* \brief Operateur AND
*/
Numerique Numerique::operatorAND(const Numerique& n)
{
if ( numReel>0 && n.numReel>0)
{
Numerique res1(1,"entier");
return res1;
}
else if (typeIm!="null" && n.typeIm!="null" && numIm>0 && n.numIm>0 )
{
Numerique res1cmplx(1,"entier");
return res1cmplx;
}
else if (numReel>0 && n.typeIm!="null" && n.numIm>0 )
{
Numerique res1cmplx(1,"entier");
return res1cmplx;
}
else if (n.numReel>0 && typeIm!="null" && numIm>0 )
{
Numerique res1cmplx(1,"entier");
return res1cmplx;
}
else
{
Numerique res0(0,"entier");
return res0;
}
}
void FuncTest::CalcHistTest()
{
std::vector<cv::Point2f> votes;
votes.push_back(cv::Point2f(0, -0.1));
votes.push_back(cv::Point2f(0.78, 2.01));
votes.push_back(cv::Point2f(-1, 0.1));
votes.push_back(cv::Point2f(0.05, 0.3));
votes.push_back(cv::Point2f(0, 0));
int treshold = 2;
int arr[] = {0, 1, 2, 3, 4};
std::vector<int> res0(arr, arr + sizeof(arr) / sizeof(int));
std::vector<int> res = calcHist(votes, treshold);
QCOMPARE(res0, res);
treshold = 1;
int arr2[] = {0, 3, 4};
res0 = std::vector<int>(arr2, arr2 + sizeof(arr2) / sizeof(int));
res = calcHist(votes, treshold);
QCOMPARE(res0, res);
treshold = 0;
res0.clear();
res = calcHist(votes, treshold);
QCOMPARE(res0.size(), res.size());
}
int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt)
{
//calculate 4 possible cases areas, and take biggest area
//also need to keep 'deepest'
int maxPenetrationIndex = -1;
#define KEEP_DEEPEST_POINT 1
#ifdef KEEP_DEEPEST_POINT
btScalar maxPenetration = pt.getDistance();
for (int i=0;i<4;i++)
{
if (m_pointCache[i].getDistance() < maxPenetration)
{
maxPenetrationIndex = i;
maxPenetration = m_pointCache[i].getDistance();
}
}
#endif //KEEP_DEEPEST_POINT
btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
if (maxPenetrationIndex != 0)
{
btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
btVector3 cross = a0.cross(b0);
res0 = cross.length2();
}
if (maxPenetrationIndex != 1)
{
btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
btVector3 cross = a1.cross(b1);
res1 = cross.length2();
}
if (maxPenetrationIndex != 2)
{
btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
btVector3 cross = a2.cross(b2);
res2 = cross.length2();
}
if (maxPenetrationIndex != 3)
{
btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
btVector3 cross = a3.cross(b3);
res3 = cross.length2();
}
btVector4 maxvec(res0,res1,res2,res3);
int biggestarea = maxvec.closestAxis4();
return biggestarea;
}
/**
* \fn Numerique Numerique::operator>(const Numerique& n)
* \brief Operateur >
*/
Numerique Numerique::operator>(const Numerique& n)
{
double temp1=(double)numReel/(double)denomReel;
double temp2=(double)n.numReel/(double)n.denomReel;
if (temp1 > temp2)
{
Numerique res1(1,"entier");
return res1;
}
else
{
Numerique res0(0,"entier");
return res0;
}
}
/**
* \fn Numerique Numerique::operator!=(const Numerique& n)
* \brief Operateur !=
*/
Numerique Numerique::operator!=(const Numerique& n)
{
if (numReel!=n.numReel || denomReel!=n.denomReel || numIm!=n.numIm || denomIm!=n.denomIm)
{
Numerique res1(1,"entier");
return res1;
}
else
{
Numerique res0(0,"entier");
return res0;
}
}
/**
* \fn Numerique Numerique::operator==(const Numerique& n)
* \brief Operateur ==
*/
Numerique Numerique::operator==(const Numerique& n)
{
if (numReel==n.numReel && denomReel==n.denomReel && numIm==n.numIm && denomIm==n.denomIm)
{
Numerique res1(1,"entier");
return res1;
}
else
{
Numerique res0(0,"entier");
return res0;
}
}
void FuncTest::in1dTest()
{
int arrA[] = {1, 2, 3, 4, 5};
int arrB[3][4] = {{1, 1, 3, 2},{5, 11, 12, 12},{10, 11, 12, 13}};
bool binArr[3][5] = {{true, true, true, false, false},
{false, false, false, false, true},
{false, false, false, false, false}};
for(int i = 0; i < 3; i++) {
std::vector<int> a(arrA, arrA + sizeof(arrA) / sizeof(int));
std::vector<int> b(arrB[i], arrB[i] + sizeof(arrB[i]) / sizeof(int));
std::vector<bool> res0(binArr[i], binArr[i] + sizeof(binArr[i]) / sizeof(bool));
std::vector<bool> res = in1d(a, b);
QCOMPARE(res0, res);
}
}
/**
* \fn Numerique Numerique::operatorNOT()
* \brief Operateur NOT
*/
Numerique Numerique::operatorNOT()
{
if ( numReel>0)
{
Numerique res0(0,"entier");
return res0;
}
if ( numIm>0)
{
Numerique res0cmplx(0,"entier");
return res0cmplx;
}
else
{
Numerique res1(1,"entier");
return res1;
}
}
void FuncTest::findPointsTest()
{
std::vector<std::vector<int> > pairs;
for (int i = 0; i < 5; i++) {
std::vector<int> list;
pairs.push_back(list);
}
pairs[0].push_back(1);
pairs[0].push_back(2);
pairs[1].push_back(0);
pairs[1].push_back(2);
pairs[2].push_back(1);
pairs[2].push_back(3);
pairs[2].push_back(4);
pairs[3].push_back(2);
pairs[3].push_back(4);
pairs[4].push_back(0);
int idx = 1;
int arr[] = {0, 2, 1, 3, 4};
std::vector<int> res0(arr, arr + sizeof(arr) / sizeof(int));
std::vector<int> res = findPoints(pairs, idx);
QCOMPARE(res0, res);
for (int i = 0; i < 5; i++) {
pairs[i].clear();
}
pairs[0].push_back(0);
pairs[0].push_back(2);
pairs[1].push_back(3);
pairs[2].push_back(2);
pairs[2].push_back(0);
int arr2[] = {3};
res0 = std::vector<int>(arr2, arr2 + sizeof(arr2) / sizeof(int));
res = findPoints(pairs, idx);
QCOMPARE(res0, res);
for (int i = 0; i < 5; i++) {
pairs[i].clear();
}
pairs[0].push_back(1);
pairs[1].push_back(2);
pairs[2].push_back(0);
idx = 0;
int arr3[] = {1, 2, 0};
res0 = std::vector<int>(arr3, arr3 + sizeof(arr3) / sizeof(int));
res = findPoints(pairs, idx);
QCOMPARE(res0, res);
for (int i = 0; i < 5; i++) {
pairs[i].clear();
}
pairs[0].push_back(2);
pairs[1].push_back(2);
pairs[1].push_back(0);
pairs[2].push_back(0);
pairs[2].push_back(1);
pairs[3].push_back(4);
pairs[4].push_back(3);
idx = 2;
int arr5[] = {0, 1, 2};
res0 = std::vector<int>(arr5, arr5 + sizeof(arr5) / sizeof(int));
res = findPoints(pairs, idx);
QCOMPARE(res0, res);
idx = 3;
int arr4[] = {4, 3};
res0 = std::vector<int>(arr4, arr4 + sizeof(arr4) / sizeof(int));
res = findPoints(pairs, idx);
QCOMPARE(res0, res);
}
void SIMPLE_Agents::get_IR_reading( vector <double> &_reading){
double x,z,X,Z,rotation = 0.0;
this->pos = this->get_pos();
double y = pos[1] + 0.011;
btMatrix3x3 m = btMatrix3x3(body->getWorldTransform().getRotation());
double rfPAngle = btAsin(-m[1][2]);
if ( rfPAngle < SIMD_HALF_PI ) {
if ( rfPAngle > -SIMD_HALF_PI ) rotation = btAtan2(m[0][2],m[2][2]);
else rotation = -btAtan2(-m[0][1],m[0][0]);
}
else rotation = btAtan2(-m[0][1],m[0][0]);
// IR0 reading
x = pos[0]+((robot_radius) * cos(-rotation + 0.3)); //calc IR sensor X based on robot radius
z = pos[2]+((robot_radius) * sin(-rotation + 0.3));
from1[0] = btVector3(x, y, z); //sensor position based on robot rotation vector to hold btVector
X = x+((IR_range) * cos(-rotation + 0.3 )); //calc IR ray end position based on IR range and placement of sensor on the robot
Z = z+((IR_range) * sin(-rotation + 0.3 ));
to1[0] = btVector3( X, y, Z ); //Max reading pos of the IR based on rotation and IR range
btCollisionWorld::ClosestRayResultCallback res0(from1[0], to1[0]); //struct for the closest ray callback
//uncomment below line if you experience any ray pentration problem to the object this might happened with very slow machine
//res0.m_flags = 0xFFFFFFFF;
this->world->rayTest(from1[0], to1[0], res0); //check for ray collision between the coords sets
if(res0.hasHit()){
_reading[0] = IR_range*res0.m_closestHitFraction ;
to1[0]=res0.m_hitPointWorld; //update the vector with the btVector results -> used to render it in openGL
}
// IR7 reading
x = pos[0]+((robot_radius) * cos(-rotation - 0.3));
z = pos[2]+((robot_radius) * sin(-rotation - 0.3));
from1[1] = btVector3(x, y, z);
X = x+((IR_range) * cos(-rotation - 0.3 ));
Z = z+((IR_range) * sin(-rotation - 0.3 ));
to1[1] = btVector3( X, y, Z );
btCollisionWorld::ClosestRayResultCallback res7(from1[1], to1[1]);
//uncomment below line if you experience any ray pentration problem to the object this might happened with very slow machine
//res7.m_flags = 0xFFFFFFFF;
this->world->rayTest(from1[1], to1[1], res7);
if(res7.hasHit()){
_reading[1] = IR_range*res7.m_closestHitFraction;
to1[1]=res7.m_hitPointWorld;
}
// IR1 reading corrospond to epuck
x = pos[0]+((robot_radius) * cos(-rotation + 0.8));
z = pos[2]+((robot_radius) * sin(-rotation + 0.8));
from1[2] = btVector3(x, y, z);
X = x+((IR_range) * cos(-rotation + 0.8 ));
Z = z+((IR_range) * sin(-rotation + 0.8 ));
to1[2] = btVector3( X, y, Z );
btCollisionWorld::ClosestRayResultCallback res1(from1[2], to1[2]);
//uncomment below line if you experience any ray pentration problem to the object this might happened with very slow machine
//res1.m_flags = 0xFFFFFFFF;
this->world->rayTest(from1[2], to1[2], res1);
if(res1.hasHit()){
_reading[2] = IR_range*res1.m_closestHitFraction;
to1[2]=res1.m_hitPointWorld;
}
// IR6 reading
x = pos[0]+((robot_radius) * cos(-rotation - 0.8));
z = pos[2]+((robot_radius) * sin(-rotation - 0.8));
from1[3] = btVector3(x, y, z);
X = x+((IR_range) * cos(-rotation - 0.8 ));
Z = z+((IR_range) * sin(-rotation - 0.8 ));
to1[3] = btVector3( X, y, Z );
btCollisionWorld::ClosestRayResultCallback res6(from1[3], to1[3]);
//uncomment below line if you experience any ray pentration problem to the object this might happened with very slow machine
//res6.m_flags = 0xFFFFFFFF;
this->world->rayTest(from1[3], to1[3], res6);
if(res6.hasHit()){
_reading[3] = IR_range*res6.m_closestHitFraction;
to1[3]=res6.m_hitPointWorld;
}
// IR2 reading
x = pos[0]+((0.028) * cos(-rotation + 1.57));
z = pos[2]+((0.028) * sin(-rotation + 1.57));
from1[4] = btVector3(x, y, z);
X = x+((0.049) * cos(-rotation + 1.57 ));
Z = z+((0.049) * sin(-rotation + 1.57));
to1[4] = btVector3( X, y, Z );
btCollisionWorld::ClosestRayResultCallback res2(from1[4], to1[4]);
//uncomment below line if you experience any ray pentration problem to the object this might happened with very slow machine
//res2.m_flags = 0xFFFFFFFF;
this->world->rayTest(from1[4], to1[4], res2);
if(res2.hasHit()){
_reading[4] = 0.049*(res2.m_closestHitFraction) - 0.009;
to1[4]=res2.m_hitPointWorld;
}
// IR5 reading
x = pos[0]+((0.028) * cos(-rotation - 1.57));
z = pos[2]+((0.028) * sin(-rotation - 1.57));
from1[5] = btVector3(x, y, z);
X = x+((0.049) * cos(-rotation - 1.57 ));
Z = z+((0.049) * sin(-rotation - 1.57 ));
to1[5] = btVector3( X, y, Z );
btCollisionWorld::ClosestRayResultCallback res5(from1[5], to1[5]);
//uncomment below line if you experience any ray pentration problem to the object this might happened with very slow machine
//res5.m_flags = 0xFFFFFFFF;
this->world->rayTest(from1[5], to1[5], res5);
if(res5.hasHit()){
_reading[5] = 0.049*res5.m_closestHitFraction - 0.009;
to1[5]=res5.m_hitPointWorld;
}
// IR3 reading
x = pos[0]+((robot_radius) * cos(-rotation + 2.64));
z = pos[2]+((robot_radius) * sin(-rotation + 2.64));
from1[6] = btVector3(x, y, z);
X = x+((IR_range) * cos(-rotation + 2.64 ));
Z = z+((IR_range) * sin(-rotation + 2.64 ));
to1[6] = btVector3( X, y, Z );
btCollisionWorld::ClosestRayResultCallback res3(from1[6], to1[6]);
//uncomment below line if you experience any ray pentration problem to the object this might happened with very slow machine
//res3.m_flags = 0xFFFFFFFF;
this->world->rayTest(from1[6], to1[6], res3);
if(res3.hasHit()){
_reading[6] = IR_range*res3.m_closestHitFraction;
to1[6]=res3.m_hitPointWorld;
}
// IR4 reading
x = pos[0]+((robot_radius) * cos(-rotation - 2.64));
z = pos[2]+((robot_radius) * sin(-rotation - 2.64));
from1[7] = btVector3(x, y, z);
X = x+((IR_range) * cos(-rotation - 2.64 ));
Z = z+((IR_range) * sin(-rotation - 2.64 ));
to1[7] = btVector3( X, y, Z );
btCollisionWorld::ClosestRayResultCallback res4(from1[7], to1[7]);
//uncomment below line if you experience any ray pentration problem to the object this might happened with very slow machine
//res4.m_flags = 0xFFFFFFFF;
this->world->rayTest(from1[7], to1[7], res4);
if(res4.hasHit()){
_reading[7] = IR_range*res4.m_closestHitFraction;
to1[7]=res4.m_hitPointWorld;
}
// for( int i = 0; i < num_IR_sensors; i++){
// printf(" \nIR%d distance reading= %f ",i,_reading[i]);
// }
//calibrating distance to IR value reading according to a line equations
for(int i = 0; i < num_IR_sensors; i++){
if (_reading[i] > 0.03 && _reading[i] <= 0.04)
_reading[i] = -20600 * _reading[i] + 924;
else if (_reading[i] > 0.02 && _reading[i] <= 0.03)
_reading[i] = -37000 * _reading[i] + 1416;
else if (_reading[i] > 0.01 && _reading[i] <= 0.02)
_reading[i] = -153500 * _reading[i] + 3746;
else if (_reading[i] > 0.005 && _reading[i] <= 0.01)
_reading[i] = -252600 * _reading[i] + 4737;
else if (_reading[i] >= 0.0 && _reading[i] <= 0.005 )
_reading[i] = -124200 * _reading[i] + 4095;
}
// for( int i = 0; i < num_IR_sensors; i++){
// printf("\n IR%d distance reading= %f ",i,_reading[i]);
// }
// take_occupancy_reading(_reading, get_rotation(), get_pos()[0], get_pos()[2] );
}
