Ball colliding with chassis, but not interfering on kicker/ball contact

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();

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;i<surfaces.count();i++)

src/sslworld.cpp

@@ -115,21 +115,34 @@ bool rayCallback(dGeomID o1,dGeomID o2,PSurface* s, int robots_count)
 
 bool ballCallBack(dGeomID o1,dGeomID o2,PSurface* s, int /*robots_count*/)
 {
-    if (_w->ball->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)
         {