@@ -31,8 +31,8 @@ using namespace camun::simulator;
3131
3232const float MAX_SPEED = 1000 ;
3333
34- SimRobot::SimRobot (const robot::Specs & specs,
35- std::shared_ptr<btDiscreteDynamicsWorld> world, const btVector3 & pos,
34+ SimRobot::SimRobot (const robot::Specs& specs,
35+ std::shared_ptr<btDiscreteDynamicsWorld> world, const btVector3& pos,
3636 float dir)
3737 : m_specs(specs),
3838 m_world(world),
@@ -54,11 +54,11 @@ SimRobot::SimRobot(const robot::Specs &specs,
5454 createRobotMesh (m_specs.radius () - COLLISION_MARGIN / SIMULATOR_SCALE,
5555 m_specs.height () - 2 * COLLISION_MARGIN / SIMULATOR_SCALE,
5656 m_specs.angle (), 0 .04f , m_specs.dribbler_height () + 0 .02f );
57- for (const auto & hullPart : mesh)
57+ for (const auto & hullPart : mesh)
5858 {
5959 std::unique_ptr<btConvexHullShape> hullPartShape =
6060 std::make_unique<btConvexHullShape>();
61- for (const auto & [x, y, z] : hullPart)
61+ for (const auto & [x, y, z] : hullPart)
6262 {
6363 hullPartShape->addPoint (btVector3 (x, y, z) * SIMULATOR_SCALE);
6464 }
@@ -158,7 +158,7 @@ void SimRobot::calculateDribblerMove(const btVector3 pos, const btQuaternion rot
158158 m_dribblerBody->setAngularVelocity (btVector3 (0 , 0 , 0 ));
159159}
160160
161- void SimRobot::dribble (const SimBall & ball, float speed)
161+ void SimRobot::dribble (const SimBall& ball, float speed)
162162{
163163 if (m_perfectDribbler)
164164 {
@@ -210,7 +210,7 @@ void SimRobot::setDribbleMode(bool perfectDribbler)
210210 m_perfectDribbler = perfectDribbler;
211211}
212212
213- void SimRobot::begin (SimBall & ball, double time)
213+ void SimRobot::begin (SimBall& ball, double time)
214214{
215215 m_commandTime += time;
216216 m_inStandby = false ;
@@ -235,7 +235,7 @@ void SimRobot::begin(SimBall &ball, double time)
235235 stopDribbling ();
236236 }
237237
238- auto sendPartialCoordError = [this ](const std::string & msg)
238+ auto sendPartialCoordError = [this ](const std::string& msg)
239239 {
240240 std::cerr << " Partial coordinates are not implemented yet" << msg << std::endl;
241241 if (!m_move.has_by_force () || !m_move.by_force ())
@@ -335,7 +335,7 @@ void SimRobot::begin(SimBall &ball, double time)
335335 }
336336 else
337337 {
338- const auto & transform = m_body->getWorldTransform ();
338+ const auto & transform = m_body->getWorldTransform ();
339339 rot = transform.getRotation ();
340340 pos = transform.getOrigin ();
341341 }
@@ -543,7 +543,7 @@ float SimRobot::bound(float acceleration, float oldSpeed, float speedupLimit,
543543 }
544544}
545545
546- btVector3 SimRobot::relativeBallSpeed (const SimBall & ball) const
546+ btVector3 SimRobot::relativeBallSpeed (const SimBall& ball) const
547547{
548548 btTransform t = m_body->getWorldTransform ();
549549 const btVector3 ballSpeed = ball.speed ();
@@ -554,7 +554,7 @@ btVector3 SimRobot::relativeBallSpeed(const SimBall &ball) const
554554 return diff;
555555}
556556
557- bool SimRobot::canKickBall (const SimBall & ball) const
557+ bool SimRobot::canKickBall (const SimBall& ball) const
558558{
559559 const btVector3 ballPos = ball.position ();
560560 // can't kick jumping ball
@@ -572,17 +572,17 @@ bool SimRobot::canKickBall(const SimBall &ball) const
572572 int numManifolds = m_world->getDispatcher ()->getNumManifolds ();
573573 for (int i = 0 ; i < numManifolds; ++i)
574574 {
575- btPersistentManifold * contactManifold =
575+ btPersistentManifold* contactManifold =
576576 m_world->getDispatcher ()->getManifoldByIndexInternal (i);
577- btCollisionObject * objectA = (btCollisionObject *)(contactManifold->getBody0 ());
578- btCollisionObject * objectB = (btCollisionObject *)(contactManifold->getBody1 ());
577+ btCollisionObject* objectA = (btCollisionObject*)(contactManifold->getBody0 ());
578+ btCollisionObject* objectB = (btCollisionObject*)(contactManifold->getBody1 ());
579579 if ((objectA == m_dribblerBody.get () && objectB == ball.body ()) ||
580580 (objectA == ball.body () && objectB == m_dribblerBody.get ()))
581581 {
582582 int numContacts = contactManifold->getNumContacts ();
583583 for (int j = 0 ; j < numContacts; ++j)
584584 {
585- btManifoldPoint & pt = contactManifold->getContactPoint (j);
585+ btManifoldPoint& pt = contactManifold->getContactPoint (j);
586586 if (pt.getDistance () < 0 .001f * SIMULATOR_SCALE)
587587 {
588588 return true ;
@@ -593,8 +593,8 @@ bool SimRobot::canKickBall(const SimBall &ball) const
593593 return false ;
594594}
595595
596- robot::RadioResponse SimRobot::setCommand (const SSLSimulationProto::RobotCommand & command,
597- const SimBall & ball, bool charge, float rxLoss,
596+ robot::RadioResponse SimRobot::setCommand (const SSLSimulationProto::RobotCommand& command,
597+ const SimBall& ball, bool charge, float rxLoss,
598598 float txLoss)
599599{
600600 m_sslCommand = command;
@@ -619,15 +619,15 @@ robot::RadioResponse SimRobot::setCommand(const SSLSimulationProto::RobotCommand
619619 float v_s = v_local.x () / SIMULATOR_SCALE;
620620 float omega = m_body->getAngularVelocity ().z ();
621621
622- robot::SpeedStatus * speedStatus = response.mutable_estimated_speed ();
622+ robot::SpeedStatus* speedStatus = response.mutable_estimated_speed ();
623623 speedStatus->set_v_f (v_f);
624624 speedStatus->set_v_s (v_s);
625625 speedStatus->set_omega (omega);
626626
627627 return response;
628628}
629629
630- void SimRobot::update (SSLProto::SSL_DetectionRobot & robot, float stddev_p,
630+ void SimRobot::update (SSLProto::SSL_DetectionRobot& robot, float stddev_p,
631631 float stddev_phi, int64_t time, btVector3 positionOffset)
632632{
633633 // setup vision packet
@@ -651,7 +651,7 @@ void SimRobot::update(SSLProto::SSL_DetectionRobot &robot, float stddev_p,
651651 m_lastSendTime = time;
652652}
653653
654- bool SimRobot::touchesBall (const SimBall & ball) const
654+ bool SimRobot::touchesBall (const SimBall& ball) const
655655{
656656 // for some reason btHingeConstraints, which is used when dribbling, are not always
657657 // detected as contact by bullet. so if the ball is being dribbled then we assume it
@@ -666,12 +666,12 @@ bool SimRobot::touchesBall(const SimBall &ball) const
666666 int num_manifolds = m_world->getDispatcher ()->getNumManifolds ();
667667 for (int i = 0 ; i < num_manifolds; ++i)
668668 {
669- btPersistentManifold * contact_manifold =
669+ btPersistentManifold* contact_manifold =
670670 m_world->getDispatcher ()->getManifoldByIndexInternal (i);
671671
672672 // determine if the two objects are the ball and robot body/dribbler
673- btCollisionObject * objectA = (btCollisionObject *)(contact_manifold->getBody0 ());
674- btCollisionObject * objectB = (btCollisionObject *)(contact_manifold->getBody1 ());
673+ btCollisionObject* objectA = (btCollisionObject*)(contact_manifold->getBody0 ());
674+ btCollisionObject* objectB = (btCollisionObject*)(contact_manifold->getBody1 ());
675675 if ((objectA == m_dribblerBody.get () && objectB == ball.body ()) ||
676676 (objectA == ball.body () && objectB == m_dribblerBody.get ()) ||
677677 (objectA == m_body.get () && objectB == ball.body ()) ||
@@ -681,7 +681,7 @@ bool SimRobot::touchesBall(const SimBall &ball) const
681681 int num_contacts = contact_manifold->getNumContacts ();
682682 for (int j = 0 ; j < num_contacts; ++j)
683683 {
684- btManifoldPoint & pt = contact_manifold->getContactPoint (j);
684+ btManifoldPoint& pt = contact_manifold->getContactPoint (j);
685685 if (pt.getDistance () < 0 .001f * SIMULATOR_SCALE)
686686 {
687687 return true ;
@@ -693,7 +693,7 @@ bool SimRobot::touchesBall(const SimBall &ball) const
693693 return false ;
694694}
695695
696- void SimRobot::update (world::SimRobot & robot, const SimBall & ball) const
696+ void SimRobot::update (world::SimRobot& robot, const SimBall& ball) const
697697{
698698 btTransform transform;
699699 m_motionState->getWorldTransform (transform);
@@ -704,7 +704,7 @@ void SimRobot::update(world::SimRobot &robot, const SimBall &ball) const
704704 robot.set_id (m_specs.id ());
705705
706706 const btQuaternion q = transform.getRotation ();
707- auto * rotation = robot.mutable_rotation ();
707+ auto * rotation = robot.mutable_rotation ();
708708 rotation->set_real (q.getX ());
709709 rotation->set_i (q.getY ());
710710 rotation->set_j (q.getZ ());
@@ -731,10 +731,10 @@ void SimRobot::update(world::SimRobot &robot, const SimBall &ball) const
731731 int numManifolds = m_world->getDispatcher ()->getNumManifolds ();
732732 for (int i = 0 ; i < numManifolds; ++i)
733733 {
734- btPersistentManifold * contactManifold =
734+ btPersistentManifold* contactManifold =
735735 m_world->getDispatcher ()->getManifoldByIndexInternal (i);
736- btCollisionObject * objectA = (btCollisionObject *)(contactManifold->getBody0 ());
737- btCollisionObject * objectB = (btCollisionObject *)(contactManifold->getBody1 ());
736+ btCollisionObject* objectA = (btCollisionObject*)(contactManifold->getBody0 ());
737+ btCollisionObject* objectB = (btCollisionObject*)(contactManifold->getBody1 ());
738738 if ((objectA == m_dribblerBody.get () && objectB == ball.body ()) ||
739739 (objectA == ball.body () && objectB == m_dribblerBody.get ()) ||
740740 (objectA == m_body.get () && objectB == ball.body ()) ||
@@ -746,7 +746,7 @@ void SimRobot::update(world::SimRobot &robot, const SimBall &ball) const
746746 robot.set_touches_ball (ballTouchesRobot);
747747}
748748
749- void SimRobot::restoreState (const world::SimRobot & robot)
749+ void SimRobot::restoreState (const world::SimRobot& robot)
750750{
751751 btVector3 position (robot.p_x (), robot.p_y (), robot.p_z ());
752752 m_body->getWorldTransform ().setOrigin (position * SIMULATOR_SCALE);
@@ -759,7 +759,7 @@ void SimRobot::restoreState(const world::SimRobot &robot)
759759 m_body->setAngularVelocity (angular);
760760}
761761
762- void SimRobot::move (const sslsim::TeleportRobot & robot)
762+ void SimRobot::move (const sslsim::TeleportRobot& robot)
763763{
764764 m_move = robot;
765765}
0 commit comments