bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
{
// // rot = cy*cz -cy*sz sy
// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
//
btScalar fi = btGetMatrixElem(mat,2);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
xyz[1] = btAsin(btGetMatrixElem(mat,2));
xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
return true;
}
else
{
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
xyz[1] = -SIMD_HALF_PI;
xyz[2] = btScalar(0.0);
return false;
}
}
else
{
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
xyz[1] = SIMD_HALF_PI;
xyz[2] = 0.0;
}
return false;
}
bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz)
{
// rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx
// cy*sz+cz*sx*sy cz*cx sz*sy-cz*xy*sx
// -cx*sy sx cx*cy
btScalar fi = btGetMatrixElem(mat,7);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
xyz[0] = btAsin(btGetMatrixElem(mat,7));
xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,4));
return true;
}
else
{
xyz[0] = -SIMD_HALF_PI;
xyz[1] = btScalar(0.0);
xyz[2] = -btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
return false;
}
}
else
{
xyz[0] = SIMD_HALF_PI;
xyz[1] = btScalar(0.0);
xyz[2] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
}
return false;
}
bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz)
{
// rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy
// cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx
// -sy cy*sx cy*cx
btScalar fi = btGetMatrixElem(mat,6);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
xyz[0] = btAtan2(btGetMatrixElem(mat,7), btGetMatrixElem(mat,8));
xyz[1] = btAsin(-btGetMatrixElem(mat,6));
xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,0));
return true;
}
else
{
xyz[0] = btScalar(0.0);
xyz[1] = SIMD_HALF_PI;
xyz[2] = -btAtan2(btGetMatrixElem(mat,1),btGetMatrixElem(mat,2));
return false;
}
}
else
{
xyz[0] = btScalar(0.0);
xyz[1] = -SIMD_HALF_PI;
xyz[2] = btAtan2(-btGetMatrixElem(mat,1),-btGetMatrixElem(mat,2));
}
return false;
}
bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz)
{
// rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy
// cx*sz cx*cz -sx
// cy*sx*sz-cz*sy sy*sz+cy*cz*sx cy*cx
btScalar fi = btGetMatrixElem(mat,5);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
xyz[0] = btAsin(-btGetMatrixElem(mat,5));
xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,8));
xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
return true;
}
else
{
xyz[0] = SIMD_HALF_PI;
xyz[1] = -btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
xyz[2] = btScalar(0.0);
return false;
}
}
else
{
xyz[0] = -SIMD_HALF_PI;
xyz[1] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
xyz[2] = 0.0;
}
return false;
}
bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz)
{
// rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx
// sz cz*cx -cz*sx
// -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx
btScalar fi = btGetMatrixElem(mat,3);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,4));
xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,0));
xyz[2] = btAsin(btGetMatrixElem(mat,3));
return true;
}
else
{
xyz[0] = btScalar(0.0);
xyz[1] = -btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
xyz[2] = -SIMD_HALF_PI;
return false;
}
}
else
{
xyz[0] = btScalar(0.0);
xyz[1] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
xyz[2] = SIMD_HALF_PI;
}
return false;
}
bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz)
{
// rot = cy*cz -sz sy*cz
// cy*cx*sz+sx*sy cx*cz sy*cx*sz-cy*sx
// cy*sx*sz-cx*sy sx*cz sy*sx*sz+cx*cy
btScalar fi = btGetMatrixElem(mat,1);
if (fi < btScalar(1.0f))
{
if (fi > btScalar(-1.0f))
{
xyz[0] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,4));
xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
xyz[2] = btAsin(-btGetMatrixElem(mat,1));
return true;
}
else
{
xyz[0] = -btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
xyz[1] = btScalar(0.0);
xyz[2] = SIMD_HALF_PI;
return false;
}
}
else
{
xyz[0] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
xyz[1] = 0.0;
xyz[2] = -SIMD_HALF_PI;
}
return false;
}
/*Returns the rotation in degrees*/
double SIMPLE_Agents::get_rotation(){
double rotation;
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]);
rotation = rotation * 180 / 3.1415926; //convert radians to degrees
return rotation;
}
double SIMPLE_Agents::get_randb_reading( vector <double> _to_robot_pos, vector <double> &_reading){
double work_range = 0.6;
randb_from = btVector3(0.0,0.0,0.0);
randb_to = btVector3(0.0,0.0,0.0);
this->pos = this->get_pos();
// get the distance between your robot and to destination robot "_to_robot_pos"
double range = sqrt(((_to_robot_pos[0]-pos[0])*(_to_robot_pos[0]-pos[0]) + (_to_robot_pos[2]-pos[2])*(_to_robot_pos[2]-pos[2])));
if(range < work_range){
_reading[0] = range;
// get the robot orienation
btMatrix3x3 m = btMatrix3x3(body->getWorldTransform().getRotation());
double rfPAngle = btAsin(-m[1][2]);
if(rfPAngle < SIMD_HALF_PI){
if(rfPAngle > -SIMD_HALF_PI) this->rotation = btAtan2(m[0][2],m[2][2]);
else this->rotation = -btAtan2(-m[0][1],m[0][0]);
}
else this->rotation = btAtan2(-m[0][1],m[0][0]);
// check the collision accross the distance between your robot and destination robot
randb_from = btVector3(_to_robot_pos[0],_to_robot_pos[1]+0.025,_to_robot_pos[2]);
randb_to = btVector3(pos[0], pos[1]+0.025, pos[2]);
btCollisionWorld::ClosestRayResultCallback res(randb_from, randb_to);
this->world->rayTest(randb_from, randb_to, res);
if(res.hasHit()){
World_Entity* object = (World_Entity*) res.m_collisionObject->getUserPointer();
if(object->get_type_id() == ROBOT && object->get_index() == this->index){
double bearing,nest_angle,robot_angle;
robot_angle =rotation;
if(robot_angle<0.0)
robot_angle = TWO_PI + robot_angle;
nest_angle = -atan2(_to_robot_pos[2]-pos[2], _to_robot_pos[0]-pos[0]);
if(nest_angle <0.0)
nest_angle = TWO_PI + nest_angle;
bearing = nest_angle - robot_angle;
if(bearing < 0.0)
bearing = TWO_PI + bearing;
//if you want to add noise bearing
bearing += (gsl_rng_uniform_pos( GSL_randon_generator::r_rand )*0.30 - 0.15);
_reading[1] = bearing;
}
randb_to =res.m_hitPointWorld;
}
}
}
void SIMPLE_Agents::get_camera_reading( vector <double> &_reading){
double x,z,X,Z,rotation = 0.0;
int found_red,found_green,rg_counter; //red for robot and green for object
this->pos = this->get_pos();
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]);
x = pos[0]+(robot_radius * cos(-rotation));
z = pos[2]+(robot_radius * sin(-rotation));
btVector3 from(x, pos[1]+0.007, z);
from2 = from;
for(int s=0; s < num_camera_sectors*num_camera_rays_per_sectors; s++)
to2[s] = from;
for(int s=0; s < num_camera_sectors; s++){
found_red = found_green =rg_counter = 0;
for(int r=0; r < num_camera_rays_per_sectors; r++){
X = x +(camera_ray * cos(-rotation - 0.235619449 + ((r + s*num_camera_rays_per_sectors)*0.0261799388)));
Z = z +(camera_ray * sin(-rotation - 0.235619449 + ((r + s*num_camera_rays_per_sectors)*0.0261799388)));
btVector3 to( X, pos[1], Z );
btCollisionWorld::ClosestRayResultCallback res(from, to);
//uncomment below line if you experience any ray pentration problem to the object this might happened with very slow machine
//res.m_flags = 0xFFFFFFFF;
this->world->rayTest(from, to, res);
to2[r + s*num_camera_rays_per_sectors] = to;
if(res.hasHit()){
if(camera_ray*res.m_closestHitFraction < 0.005){
to2[r + s*num_camera_rays_per_sectors] = res.m_hitPointWorld;
break;
}else{
to2[r + s*num_camera_rays_per_sectors] = res.m_hitPointWorld;
World_Entity* object1 = (World_Entity*) res.m_collisionObject->getUserPointer();
if(!found_red){
if(object1->get_type_id() == ROBOT /*&& object1->get_index() == 0*/)
{
found_red = 1;
rg_counter++;
if(rg_counter > 1){
_reading[num_camera_sectors-s-1] = 1.0; // the robot see red and green
break;
}
else{
_reading[num_camera_sectors-s-1] = 0.4; // the robot see only red color
continue;
}
}
}
if(!found_green){
if(object1->get_type_id() == 4 /*&& object1->get_index() == 1*/) //object
{
found_green = 1;
rg_counter++;
if(rg_counter > 1){
_reading[num_camera_sectors-s-1] = 1.0; // the robot see both red and green color
break;
}
else{
_reading[num_camera_sectors-s-1] = 0.7; // the robot see only green color
continue;
}
}
}
}
}
}
}
}
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] );
}