From 0708d9f56f98c40b2720b46036b4a424c33f98cf Mon Sep 17 00:00:00 2001 From: Felipe Date: Wed, 3 Mar 2021 20:30:39 -0300 Subject: [PATCH 1/3] Ball colliding with chassis, but not interfering on kicker/ball contact Co-authored-by: Victor Sabino --- include/physics/pworld.h | 1 + src/physics/pworld.cpp | 10 ++++++++ src/sslworld.cpp | 49 +++++++++++++++++++++++++--------------- 3 files changed, 42 insertions(+), 18 deletions(-) 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 *pos_ball = dBodyGetPosition(body); + body = dGeomGetBody(o2); + const dReal *pos_robot = dBodyGetPosition(body); + const dReal *dir_robot = dBodyGetRotation(body); + + // Get robot angle + dVector3 v={1,0,0}; + dVector3 axis; + dMultiply0(axis,dir_robot,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 angle_robot = (axis[1] > 0) ? absAng : -absAng; + + // Get angle between robot and ball + dReal angle_robot_ball = atan((pos_ball[1] - pos_robot[1])/(pos_ball[0]-pos_robot[0])); + angle_robot_ball = (pos_ball[0] > pos_robot[0]) ? angle_robot_ball : M_PI - angle_robot_ball; + + dReal angle_kicker = 0.625; + + dReal angle_diff = abs(angle_robot_ball - angle_robot); + + return (angle_diff < angle_kicker) ? false : true; } SSLWorld::SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1, RobotsFormation *form2) : QObject(parent) { @@ -267,7 +276,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 +289,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) { From d5da59ff2191aaaf1173a0c4a5c8c87e862143f5 Mon Sep 17 00:00:00 2001 From: Felipe Date: Wed, 3 Mar 2021 20:55:57 -0300 Subject: [PATCH 2/3] basic formatting changes --- src/sslworld.cpp | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/src/sslworld.cpp b/src/sslworld.cpp index 11b78399..f9d7fd80 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -116,29 +116,32 @@ bool rayCallback(dGeomID o1,dGeomID o2,PSurface* s, int robots_count) bool ballCallBack(dGeomID o1,dGeomID o2,PSurface* s, int /*robots_count*/) { auto body = dGeomGetBody(o1); - const dReal *pos_ball = dBodyGetPosition(body); + const dReal *posBall = dBodyGetPosition(body); body = dGeomGetBody(o2); - const dReal *pos_robot = dBodyGetPosition(body); - const dReal *dir_robot = dBodyGetRotation(body); + const dReal *posRobot = dBodyGetPosition(body); + const dReal *dirRobot = dBodyGetRotation(body); // Get robot angle dVector3 v={1,0,0}; dVector3 axis; - dMultiply0(axis,dir_robot,v,4,3,1); + 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 angle_robot = (axis[1] > 0) ? absAng : -absAng; + dReal angleRobot = (axis[1] > 0) ? absAng : -absAng; // Get angle between robot and ball - dReal angle_robot_ball = atan((pos_ball[1] - pos_robot[1])/(pos_ball[0]-pos_robot[0])); - angle_robot_ball = (pos_ball[0] > pos_robot[0]) ? angle_robot_ball : M_PI - angle_robot_ball; + dReal angleRobotBall = atan((posBall[1] - posRobot[1])/(posBall[0]-posRobot[0])); + angleRobotBall = (posBall[0] > posRobot[0]) ? angleRobotBall : M_PI - angleRobotBall; - dReal angle_kicker = 0.625; + // This value is given by the acos(distance_center_kicker/robot_radius) + dReal angleKicker = 0.625; - dReal angle_diff = abs(angle_robot_ball - angle_robot); + dReal angleDiff = abs(angleRobotBall - angleRobot); - return (angle_diff < angle_kicker) ? false : true; + // If kicker is facing the ball, the collision with the chassis should not + // be considered + return (angleDiff < angleKicker) ? false : true; } SSLWorld::SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1, RobotsFormation *form2) : QObject(parent) { From e55ae410b07d02247f55f59289378e1141c2a465 Mon Sep 17 00:00:00 2001 From: Felipe Date: Thu, 11 Mar 2021 15:10:56 -0300 Subject: [PATCH 3/3] fixes to trigonometric calculations --- src/sslworld.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/sslworld.cpp b/src/sslworld.cpp index f9d7fd80..7ee1fd36 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -131,17 +131,18 @@ bool ballCallBack(dGeomID o1,dGeomID o2,PSurface* s, int /*robots_count*/) dReal angleRobot = (axis[1] > 0) ? absAng : -absAng; // Get angle between robot and ball - dReal angleRobotBall = atan((posBall[1] - posRobot[1])/(posBall[0]-posRobot[0])); - angleRobotBall = (posBall[0] > posRobot[0]) ? angleRobotBall : M_PI - angleRobotBall; + 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; - dReal angleDiff = abs(angleRobotBall - angleRobot); + // 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 (angleDiff < angleKicker) ? false : true; + return (abs(angleDiff) < angleKicker) ? false : true; } SSLWorld::SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1, RobotsFormation *form2) : QObject(parent) {