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 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::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;
}
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;
}
/*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;
}
void btGpuDemoDynamicsWorld::grabData()
{
BT_PROFILE("grab data");
m_numObj = getNumCollisionObjects();
m_hPos[0].x = m_hPos[0].y = m_hPos[0].z = m_hPos[0].w = 0.f;
m_hRot[0] = 0.f;
m_hVel[0].x = m_hVel[0].y = m_hVel[0].z = m_hVel[0].w = 0.f;
m_hAngVel[0] = 0.f;
for(int i = 0; i < m_numObj; i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* rb = btRigidBody::upcast(colObj);
btVector3 v;
v = rb->getCenterOfMassPosition();
m_hPos[i+1] = *((float4*)&v);
const btTransform& tr = rb->getCenterOfMassTransform();
v = tr.getBasis().getColumn(0);
float rot = btAtan2(v[1], v[0]);
m_hRot[i+1] = rot;
v = rb->getLinearVelocity();
m_hVel[i+1] = *((float4*)&v);
v = rb->getAngularVelocity();
m_hAngVel[i+1] = v[2];
if(m_copyMassDataToGPU)
{
m_hInvMass[i+1] = rb->getInvMass();
}
}
if(m_useBulletNarrowphase)
{
grabContactData();
}
grabNonContactConstraintData();
}
void btSliderConstraint::testAngLimits(void)
{
m_angDepth = btScalar(0.);
m_solveAngLim = false;
if(m_lowerAngLimit <= m_upperAngLimit)
{
const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
// btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0));
rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
m_angPos = rot;
if(rot < m_lowerAngLimit)
{
m_angDepth = rot - m_lowerAngLimit;
m_solveAngLim = true;
}
else if(rot > m_upperAngLimit)
{
m_angDepth = rot - m_upperAngLimit;
m_solveAngLim = true;
}
}
}
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;
}
}
}
static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA,const btVector3& normalB)
{
const btVector3 refAxis0 = edgeA;
const btVector3 refAxis1 = normalA;
const btVector3 swingAxis = normalB;
btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
return angle;
}
btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
{
const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
// btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
return m_referenceSign * angle;
}
void ExploreFrontier::findFrontiers(Costmap2DROS& costmap_) {
frontiers_.clear();
Costmap2D costmap;
costmap_.getCostmapCopy(costmap);
int idx;
int w = costmap.getSizeInCellsX();
int h = costmap.getSizeInCellsY();
int size = (w * h);
int pts = 0;
map_.info.width = w;
map_.info.height = h;
map_.set_data_size(size);
map_.info.resolution = costmap.getResolution();
map_.info.origin.position.x = costmap.getOriginX();
map_.info.origin.position.y = costmap.getOriginY();
// Find all frontiers (open cells next to unknown cells).
const unsigned char* map = costmap.getCharMap();
for (idx = 0; idx < size; idx++) {
//get the world point for the index
unsigned int mx, my;
costmap.indexToCells(idx, mx, my);
geometry_msgs::Point p;
costmap.mapToWorld(mx, my, p.x, p.y);
//check if the point has valid potential and is next to unknown space
//bool valid_point = planner_->validPointPotential(p);
//bool valid_point = planner_->computePotential(p) && planner_->validPointPotential(p);
//bool valid_point = (map[idx] < LETHAL_OBSTACLE);
//bool valid_point = (map[idx] < INSCRIBED_INFLATED_OBSTACLE );
bool valid_point = (map[idx] == FREE_SPACE);
if ((valid_point && map) &&
(((idx+1 < size) && (map[idx+1] == NO_INFORMATION)) ||
((idx-1 >= 0) && (map[idx-1] == NO_INFORMATION)) ||
((idx+w < size) && (map[idx+w] == NO_INFORMATION)) ||
((idx-w >= 0) && (map[idx-w] == NO_INFORMATION))))
{
map_.data[idx] = -128;
//ROS_INFO("Candidate Point %d.", pts++ );
} else {
map_.data[idx] = -127;
}
}
// Clean up frontiers detected on separate rows of the map
// I don't know what this means -- Travis.
idx = map_.info.height - 1;
for (unsigned int y=0; y < map_.info.width; y++) {
map_.data[idx] = -127;
idx += map_.info.height;
}
// Group adjoining map_ pixels
int segment_id = 127;
std::vector< std::vector<FrontierPoint> > segments;
for (int i = 0; i < size; i++) {
if (map_.data[i] == -128) {
std::vector<int> neighbors, candidates;
std::vector<FrontierPoint> segment;
neighbors.push_back(i);
int points_in_seg = 0;
unsigned int mx, my;
geometry_msgs::Point p_init, p;
costmap.indexToCells(i, mx, my);
costmap.mapToWorld(mx, my, p_init.x, p_init.y);
// claim all neighbors
while (neighbors.size() > 0) {
int idx = neighbors.back();
neighbors.pop_back();
btVector3 tot(0,0,0);
int c = 0;
if ((idx+1 < size) && (map[idx+1] == NO_INFORMATION)) {
tot += btVector3(1,0,0);
c++;
}
if ((idx-1 >= 0) && (map[idx-1] == NO_INFORMATION)) {
tot += btVector3(-1,0,0);
c++;
}
if ((idx+w < size) && (map[idx+w] == NO_INFORMATION)) {
tot += btVector3(0,1,0);
c++;
}
if ((idx-w >= 0) && (map[idx-w] == NO_INFORMATION)) {
tot += btVector3(0,-1,0);
c++;
}
assert(c > 0);
// Only adjust the map and add idx to segment when its near enough to start point.
// This should prevent greedy approach where all cells belong to one frontier!
costmap.indexToCells(idx, mx, my);
costmap.mapToWorld(mx, my, p.x, p.y);
float dist;
dist = sqrt( pow( p.x - p_init.x, 2 ) + pow( p.y - p_init.y, 2 ));
if ( dist <= 0.8 ){
map_.data[idx] = segment_id; // This used to be up before "assert" block above.
points_in_seg += 1;
//ROS_INFO( "Dist to start cell: %3.2f", dist );
segment.push_back(FrontierPoint(idx, tot / c));
// consider 8 neighborhood
if (((idx-1)>0) && (map_.data[idx-1] == -128))
neighbors.push_back(idx-1);
if (((idx+1)<size) && (map_.data[idx+1] == -128))
neighbors.push_back(idx+1);
if (((idx-map_.info.width)>0) && (map_.data[idx-map_.info.width] == -128))
neighbors.push_back(idx-map_.info.width);
if (((idx-map_.info.width+1)>0) && (map_.data[idx-map_.info.width+1] == -128))
neighbors.push_back(idx-map_.info.width+1);
if (((idx-map_.info.width-1)>0) && (map_.data[idx-map_.info.width-1] == -128))
neighbors.push_back(idx-map_.info.width-1);
if (((idx+(int)map_.info.width)<size) && (map_.data[idx+map_.info.width] == -128))
neighbors.push_back(idx+map_.info.width);
if (((idx+(int)map_.info.width+1)<size) && (map_.data[idx+map_.info.width+1] == -128))
neighbors.push_back(idx+map_.info.width+1);
if (((idx+(int)map_.info.width-1)<size) && (map_.data[idx+map_.info.width-1] == -128))
neighbors.push_back(idx+map_.info.width-1);
}
}
segments.push_back(segment);
ROS_INFO( "Segment %d has %d costmap cells", segment_id, points_in_seg );
segment_id--;
if (segment_id < -127)
break;
}
}
int num_segments = 127 - segment_id;
ROS_INFO("Segments: %d", num_segments );
if (num_segments <= 0)
return;
for (unsigned int i=0; i < segments.size(); i++) {
Frontier frontier;
std::vector<FrontierPoint>& segment = segments[i];
uint size = segment.size();
//we want to make sure that the frontier is big enough for the robot to fit through
// This seems like a goofy heuristic rather than fact. What happens if we don't do this...?
if (size * costmap.getResolution() < costmap.getInscribedRadius()){
ROS_INFO( "Discarding segment... too small?" );
//continue;
}
float x = 0, y = 0;
btVector3 d(0,0,0);
for (uint j=0; j<size; j++) {
d += segment[j].d;
int idx = segment[j].idx;
x += (idx % map_.info.width);
y += (idx / map_.info.width);
// x = (idx % map_.info.width);
// y = (idx / map_.info.width);
}
d = d / size;
frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x / size);
frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y / size);
// frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x);
// frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y);
frontier.pose.position.z = 0.0;
frontier.pose.orientation = tf::createQuaternionMsgFromYaw(btAtan2(d.y(), d.x()));
frontier.size = size;
geometry_msgs::Point p;
p.x = map_.info.origin.position.x + map_.info.resolution * (x); // frontier.pose.position
p.y = map_.info.origin.position.y + map_.info.resolution * (y);
if (!planner_->validPointPotential(p)){
ROS_INFO( "Discarding segment... can't reach?" );
//continue;
}
frontiers_.push_back(frontier);
}
}
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] );
}
void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
{
m_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.);
m_solveTwistLimit = false;
m_solveSwingLimit = false;
// compute rotation of A wrt B (in constraint space)
if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
{ // it is assumed that setMotorTarget() was alredy called
// and motor target m_qTarget is within constraint limits
// TODO : split rotation to pure swing and pure twist
// compute desired transforms in world
btTransform trPose(m_qTarget);
btTransform trA = transA * m_rbAFrame;
btTransform trB = transB * m_rbBFrame;
btTransform trDeltaAB = trB * trPose * trA.inverse();
btQuaternion qDeltaAB = trDeltaAB.getRotation();
btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
float swingAxisLen2 = swingAxis.length2();
if(btFuzzyZero(swingAxisLen2))
{
return;
}
m_swingAxis = swingAxis;
m_swingAxis.normalize();
m_swingCorrection = qDeltaAB.getAngle();
if(!btFuzzyZero(m_swingCorrection))
{
m_solveSwingLimit = true;
}
return;
}
{
// compute rotation of A wrt B (in constraint space)
btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
btQuaternion qAB = qB.inverse() * qA;
// split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...)
btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
{
btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
if (swingAngle > swingLimit * m_limitSoftness)
{
m_solveSwingLimit = true;
// compute limit ratio: 0->1, where
// 0 == beginning of soft limit
// 1 == hard/real limit
m_swingLimitRatio = 1.f;
if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
{
m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
(swingLimit - swingLimit * m_limitSoftness);
}
// swing correction tries to get back to soft limit
m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
// adjustment of swing axis (based on ellipse normal)
adjustSwingAxisToUseEllipseNormal(swingAxis);
// Calculate necessary axis & factors
m_swingAxis = quatRotate(qB, -swingAxis);
m_twistAxisA.setValue(0,0,0);
m_kSwing = btScalar(1.) /
(computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
}
}
else
{
// you haven't set any limits;
// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
// anyway, we have either hinge or fixed joint
btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
btVector3 target;
btScalar x = ivB.dot(ivA);
btScalar y = ivB.dot(jvA);
btScalar z = ivB.dot(kvA);
if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
{ // fixed. We'll need to add one more row to constraint
if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
{
m_solveSwingLimit = true;
m_swingAxis = -ivB.cross(ivA);
}
}
else
{
if(m_swingSpan1 < m_fixThresh)
{ // hinge around Y axis
if(!(btFuzzyZero(y)))
{
m_solveSwingLimit = true;
if(m_swingSpan2 >= m_fixThresh)
{
y = btScalar(0.f);
btScalar span2 = btAtan2(z, x);
if(span2 > m_swingSpan2)
{
x = btCos(m_swingSpan2);
z = btSin(m_swingSpan2);
}
else if(span2 < -m_swingSpan2)
{
x = btCos(m_swingSpan2);
z = -btSin(m_swingSpan2);
}
}
}
}
else
{ // hinge around Z axis
if(!btFuzzyZero(z))
{
m_solveSwingLimit = true;
if(m_swingSpan1 >= m_fixThresh)
{
z = btScalar(0.f);
btScalar span1 = btAtan2(y, x);
if(span1 > m_swingSpan1)
{
x = btCos(m_swingSpan1);
y = btSin(m_swingSpan1);
}
else if(span1 < -m_swingSpan1)
{
x = btCos(m_swingSpan1);
y = -btSin(m_swingSpan1);
}
}
}
}
target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
target.normalize();
m_swingAxis = -ivB.cross(target);
m_swingCorrection = m_swingAxis.length();
m_swingAxis.normalize();
}
}
if (m_twistSpan >= btScalar(0.f))
{
btVector3 twistAxis;
computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
if (m_twistAngle > m_twistSpan*m_limitSoftness)
{
m_solveTwistLimit = true;
m_twistLimitRatio = 1.f;
if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
{
m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
(m_twistSpan - m_twistSpan * m_limitSoftness);
}
// twist correction tries to get back to soft limit
m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
m_twistAxis = quatRotate(qB, -twistAxis);
m_kTwist = btScalar(1.) /
(computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
}
if (m_solveSwingLimit)
m_twistAxisA = quatRotate(qA, -twistAxis);
}
else
{
m_twistAngle = btScalar(0.f);
}
}
}
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 cullPoints2 (int n, btScalar p[], int m, int i0, int iret[])
{
// compute the centroid of the polygon in cx,cy
int i,j;
btScalar a,cx,cy,q;
if (n==1) {
cx = p[0];
cy = p[1];
}
else if (n==2) {
cx = 0.5*(p[0] + p[2]);
cy = 0.5*(p[1] + p[3]);
}
else {
a = 0;
cx = 0;
cy = 0;
for (i=0; i<(n-1); i++) {
q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
a += q;
cx += q*(p[i*2]+p[i*2+2]);
cy += q*(p[i*2+1]+p[i*2+3]);
}
q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
if (btFabs(a+q) > SIMD_EPSILON)
{
a = 1.f/(3.0*(a+q));
} else
{
a=BT_LARGE_FLOAT;
}
cx = a*(cx + q*(p[n*2-2]+p[0]));
cy = a*(cy + q*(p[n*2-1]+p[1]));
}
// compute the angle of each point w.r.t. the centroid
btScalar A[8];
for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx);
// search for points that have angles closest to A[i0] + i*(2*pi/m).
int avail[8];
for (i=0; i<n; i++) avail[i] = 1;
avail[i0] = 0;
iret[0] = i0;
iret++;
for (j=1; j<m; j++) {
a = j*(2*M__PI/m) + A[i0];
if (a > M__PI) a -= 2*M__PI;
btScalar maxdiff=1e9,diff;
*iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
for (i=0; i<n; i++) {
if (avail[i]) {
diff = btFabs (A[i]-a);
if (diff > M__PI) diff = 2*M__PI - diff;
if (diff < maxdiff) {
maxdiff = diff;
*iret = i;
}
}
}
avail[*iret] = 0;
iret++;
}
}
