diff --git a/include/physics/pworld.h b/include/physics/pworld.h index b5fca404..ee6dabf3 100644 --- a/include/physics/pworld.h +++ b/include/physics/pworld.h @@ -40,6 +40,7 @@ class PWorld void addObject(PObject* o); void initAllObjects(); PSurface* createSurface(PObject* o1,PObject* o2); + PSurface* createSurfaceBallChassis(PObject* o1,PObject* o2); PSurface* findSurface(PObject* o1,PObject* o2); void step(dReal dt=-1); void glinit(); diff --git a/src/physics/pworld.cpp b/src/physics/pworld.cpp index bff3ac7d..da5b631e 100644 --- a/src/physics/pworld.cpp +++ b/src/physics/pworld.cpp @@ -155,6 +155,16 @@ PSurface* PWorld::createSurface(PObject* o1,PObject* o2) return s; } +PSurface* PWorld::createSurfaceBallChassis(PObject* o1,PObject* o2) +{ + PSurface *s = new PSurface(); + s->id1 = o1->geom; + s->id2 = o2->geom; + surfaces.append(s); + sur_matrix[o2->id][o1->id] = surfaces.count() - 1; + return s; +} + PSurface* PWorld::findSurface(PObject* o1,PObject* o2) { for (int i=0;iball->tag!=-1) //spinner adjusting - { - dReal x,y,z; - _w->robots[_w->ball->tag]->chassis->getBodyDirection(x,y,z); - s->fdir1[0] = x; - s->fdir1[1] = y; - s->fdir1[2] = 0; - s->fdir1[3] = 0; - s->usefdir1 = true; - s->surface.mode = dContactMu2 | dContactFDir1 | dContactSoftCFM; - s->surface.mu = _w->cfg->BallFriction(); - s->surface.mu2 = 0.5; - s->surface.soft_cfm = 0.002; - } - return true; + auto body = dGeomGetBody(o1); + const dReal *posBall = dBodyGetPosition(body); + body = dGeomGetBody(o2); + const dReal *posRobot = dBodyGetPosition(body); + const dReal *dirRobot = dBodyGetRotation(body); + + // Get robot angle + dVector3 v={1,0,0}; + dVector3 axis; + dMultiply0(axis,dirRobot,v,4,3,1); + dReal dot = axis[0]; + dReal length = sqrt(axis[0]*axis[0] + axis[1]*axis[1]); + dReal absAng = (dReal)(acos((dReal)(dot/length))); + dReal angleRobot = (axis[1] > 0) ? absAng : -absAng; + + // Get angle between robot and ball + dReal angleRobotBall = atan2((posBall[1] - posRobot[1]),(posBall[0]-posRobot[0])); + + // This value is given by the acos(distance_center_kicker/robot_radius) + dReal angleKicker = 0.625; + + // Smallest angle diff + dReal angleDiff = angleRobotBall - angleRobot; + angleDiff += (angleDiff>M_PI) ? -2*M_PI : (angleDiff<-M_PI) ? 2*M_PI : 0; + + // If kicker is facing the ball, the collision with the chassis should not + // be considered + return (abs(angleDiff) < angleKicker) ? false : true; } SSLWorld::SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1, RobotsFormation *form2) : QObject(parent) { @@ -267,7 +280,7 @@ SSLWorld::SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1 PSurface wheelswithground; PSurface* ball_ground = p->createSurface(ball,ground); ball_ground->surface = ballwithwall.surface; - ball_ground->callback = ballCallBack; + // ball_ground->callback = ballCallBack; PSurface ballwithkicker; ballwithkicker.surface.mode = dContactApprox1; @@ -280,8 +293,12 @@ SSLWorld::SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1 { p->createSurface(robots[k]->chassis,ground); for (auto & wall : walls) p->createSurface(robots[k]->chassis,wall); - p->createSurface(robots[k]->dummy,ball); - //p->createSurface(robots[k]->chassis,ball); + + // Create surface between ball and chassis + PSurface* ballChassis = p->createSurfaceBallChassis(robots[k]->chassis, ball); + ballChassis->callback=ballCallBack; + + p->createSurface(robots[k]->kicker->box,ball)->surface = ballwithkicker.surface; for (auto & wheel : robots[k]->wheels) {