C++ Obstacle::getPosition方法代码示例

        void DistanceToObjectsReport::report(const odcore::wrapper::Time &t) {
            cerr << "Call to DistanceToObjectsReport for t = " << t.getSeconds() << "." << t.getPartialMicroseconds() << ", containing " << getFIFO().getSize() << " containers." << endl;

            // Get last EgoState.
            KeyValueDataStore &kvds = getKeyValueDataStore();
            Container c = kvds.get(opendlv::data::environment::EgoState::ID());
            EgoState es = c.getData<EgoState>();

            const uint32_t SIZE = getFIFO().getSize();
            for (uint32_t i = 0; i < SIZE; i++) {
                c = getFIFO().leave();
                cerr << "Received: " << c.toString() << endl;

                if (c.getDataType() == opendlv::data::environment::Obstacle::ID()) {
                    Obstacle o = c.getData<Obstacle>();

                    const float DISTANCE = (es.getPosition().getDistanceTo(o.getPosition()));
                    cerr << "DistanceToObjectsReport: Distance to object: " << DISTANCE << ", E: " << es.toString() << ", o: " << o.getPosition().toString() << endl;

                    // Continuously check distance.
                    m_correctDistance &= (DISTANCE > m_threshold);

                    vector<Point3> shape = o.getPolygon().getVertices();
                    Point3 head = shape.front();
                    shape.push_back(head);
                    const uint32_t NUMVERTICES = shape.size();
                    for(uint32_t j = 1; j < NUMVERTICES; j++) {
                        Point3 pA = shape.at(j-1);
                        Point3 pB = shape.at(j);

                        // TODO: Check polygonal data as well as perpendicular to all sides.
                        // Create line.
                        Line l(pA, pB);

                        // Compute perpendicular point.
                        Point3 perpendicularPoint = l.getPerpendicularPoint(es.getPosition());

                        // Compute distance between current position and perpendicular point.
                        const float DISTANCE_PP = (es.getPosition().getDistanceTo(perpendicularPoint));

                        cerr << "DistanceToObjectsReport: Distance to object's shape: " << DISTANCE_PP << ", E: " << es.toString() << ", o: " << o.getPosition().toString() << ", perpendicular point:" << perpendicularPoint.toString() << endl;

                        // Continuously check distance.
                        m_correctDistance &= (DISTANCE > m_threshold);
                    }
                }

                if (c.getDataType() == opendlv::data::environment::OtherVehicleState::ID()) {
                    OtherVehicleState o = c.getData<OtherVehicleState>();

                    const float DISTANCE = (es.getPosition().getDistanceTo(o.getPosition()));

                    // Compute distance between current position and perpendicular point.
                    cerr << "DistanceToObjectsReport: Distance to other vehicle: " << DISTANCE << ", E: " << es.toString() << ", o: " << o.getPosition().toString() << endl;

                    // Continuously check distance.
                    m_correctDistance &= (DISTANCE > m_threshold);
                }
            }
        }

void FCScene::doFrame(void)
{
	FCFighter* fighter = (FCFighter*)_player.get();
	//_camNode->setPosition(fighter->getPosition() + fighter->getAxis()*Ogre::Vector3(-0.5, 3, -1));
	////_camNode->setDirection(fighter->getAxis()*Ogre::Vector3::UNIT_Z, Ogre::Node::TS_WORLD);
	//_camNode->setOrientation(fighter->getNode()->getOrientation()*Ogre::Quaternion(0, 0, 1, 0));
	//FCKnowledge::getSingleton().setPlayerPosition(fighter->getPosition());
	DataLogger::getSingleton().getPlayerData(fighter->getID(), fighter->getPosition().ptr(), fighter->getOrientation().ptr(), fighter->getHealthPoint());
	for(unsigned int i = 0; i < fighter->getBulletList().size(); i++)
	{
		FCBullet* bullet = (FCBullet*)fighter->getBulletList()[i].get();
		if(bullet->isActive())
			DataLogger::getSingleton().getPlayerProjectileData(bullet->getID(), bullet->getPosition().ptr(), bullet->getOrientation().ptr());
	}
	for(unsigned int i = 0; i < _enemies.size(); i++)
	{
		FCFighter* enemy = (FCFighter*)_enemies[i].get();
		DataLogger::getSingleton().getBotData(enemy->getID(), enemy->getPosition().ptr(), enemy->getOrientation().ptr(), enemy->getHealthPoint());
		for(unsigned int i = 0; i < enemy->getBulletList().size(); i++)
		{
			FCBullet* bullet = (FCBullet*)enemy->getBulletList()[i].get();
			if(bullet->isActive())
				DataLogger::getSingleton().getBotProjectileData(bullet->getID(), enemy->getID(), bullet->getPosition().ptr(), bullet->getOrientation().ptr());
		}
	}
	Obstacle* obs = (Obstacle*)_obstacle1.get();
	DataLogger::getSingleton().getObstacleData(obs->getID(), obs->getPosition().ptr(), obs->getScale());
	
	obs = (Obstacle*)_obstacle2.get();
	DataLogger::getSingleton().getObstacleData(obs->getID(), obs->getPosition().ptr(), obs->getScale());
	
	obs = (Obstacle*)_obstacle3.get();
	DataLogger::getSingleton().getObstacleData(obs->getID(), obs->getPosition().ptr(), obs->getScale());

	DataLogger::getSingleton().writeTest();
}

    Matrix getVLIM(const Obstacle &obstacle, const Matrix &v_lim)
    {
        Vector xo=obstacle.getPosition();
        deque<Vector> ctrlPoints=getCtrlPointsPosition();

        Matrix VLIM=v_lim;
        for (size_t i=0; i<ctrlPoints.size(); i++)
        {
            Vector dist=xo-ctrlPoints[i];
            double d=norm(dist);
            if (d>=obstacle.radius)
            {
                dist*=1.0-obstacle.radius/d;
                d=norm(dist);
            }
            else
            {
                dist=0.0;
                d=0.0;
            }
            
            double f=1.0/(1.0+exp((d*(2.0/rho)-1.0)*alpha));
            Matrix J=chainCtrlPoints[i]->GeoJacobian().submatrix(0,2,0,chainCtrlPoints[i]->getDOF()-1);
            Vector s=J.transposed()*dist;

            double red=1.0-f;
            for (size_t j=0; j<s.length(); j++)
            {
                if (s[j]>=0.0)
                {
                    double tmp=v_lim(j,1)*red;
                    VLIM(j,1)=std::min(VLIM(j,1),tmp);
                    VLIM(j,0)=std::min(VLIM(j,0),VLIM(j,1));
                }
                else
                {
                    double tmp=v_lim(j,0)*red;
                    VLIM(j,0)=std::max(VLIM(j,0),tmp);
                    VLIM(j,1)=std::max(VLIM(j,0),VLIM(j,1));
                }
            }
        }

        return VLIM;
    }

float			RadarRenderer::transScale(const Obstacle& o)
{
  float scaleColor;
  const LocalPlayer* myTank = LocalPlayer::getMyTank();

  // Scale color so that objects that are close to tank's level are opaque
  const float zTank = myTank->getPosition()[2];
  const float zObstacle = o.getPosition()[2];
  const float hObstacle = o.getHeight();
  if (zTank >= (zObstacle + hObstacle))
    scaleColor = 1.0f - (zTank - (zObstacle + hObstacle)) / colorFactor;
  else if (zTank <= zObstacle)
    scaleColor = 1.0f - (zObstacle - zTank) / colorFactor;
  else
    scaleColor = 1.0f;

  if (scaleColor < 0.5f)
    scaleColor = 0.5f;

  return scaleColor;
}

    Matrix getVLIM(const Obstacle &obstacle, const Matrix &v_lim)
    {
        Vector xo=obstacle.getPosition();
        deque<Vector> ctrlPoints=getCtrlPointsPosition();

        Matrix VLIM=v_lim;
        for (size_t i=0; i<ctrlPoints.size(); i++)
        {
            Vector dist=xo-ctrlPoints[i];
            double d=norm(dist);
            if (d>=obstacle.radius)
                continue;
            
            double f=k*(obstacle.radius-d);
            Matrix J=chainCtrlPoints[i]->GeoJacobian().submatrix(0,2,0,chainCtrlPoints[i]->getDOF()-1);
            Vector s=(-f/d)*(J.transposed()*dist);
            
            for (size_t j=0; j<s.length(); j++)
            {
                if (s[j]>=0.0)
                {
                    s[j]=std::min(v_lim(j,1),s[j]);
                    VLIM(j,0)=std::max(VLIM(j,0),s[j]);
                    VLIM(j,1)=std::max(VLIM(j,0),VLIM(j,1));
                }
                else
                {
                    s[j]=std::max(v_lim(j,0),s[j]);
                    VLIM(j,1)=std::min(VLIM(j,1),s[j]);
                    VLIM(j,0)=std::min(VLIM(j,0),VLIM(j,1));
                }
            }
        }

        return VLIM;
    }

//.........这里部分代码省略.........

            // Daten zu history hinzufuegen fuer spaeteren plot
            if(ActorHighLevel::streamPIDEnabled) {
                mutexPidHist->lock();
                pidHistTime.append(timeSinceStart);
                pidHistWinkelIst.append(constrainAngle(robotPosition.rot()));
                pidHistWinkelSoll.append(0.0);
                pidHistDistIst.append(dist);
                pidHistDistSoll.append(deltaL);
                mutexPidHist->unlock();
            }

            // Emit the calculated control values to the low level actor module
            Q_EMIT signalSendRobotControlParams(robotV, 0.0);

            break;
        }
        case StatePathProcessing::RUNNING:
        {
            // Wenn es keine Wegpunkte gibt
            if(internalWP.isEmpty() || MapData::getObstacle(ObstacleType::TARGET).isEmpty()) {
                // Leave the state
                this->setState(StatePathProcessing::STOP);
                break;
            }

            // Turn off primitive collision avoidance
            MapData::setDisableEmergency(true);

            // Wenn der Roboter auf dem letzten Wegpunkt steht
            Obstacle target = MapData::getFirstTarget();

            bool targetHasOrientation = !std::isnan(target.getOrientation()) && !std::isinf(target.getOrientation());
            double targetDist = robotPosition.getDistanceTo(target.getPosition());
            double targetDistDiff = targetDist - targetDistLast;
            targetDistLast = targetDist;
            if(targetDistDiff == 0) targetDistDiff = targetDistDiffLast;
            targetDistDiffLast = targetDistDiff;
            bool targetSeemsReached = targetDistDiff > config::actorWaypointReachedDiffChange && targetDist <= config::actorWaypointReachedDistance;
            if( positionWasReached || targetSeemsReached ) {
                // Roboter ist/war an Position

                if(positionWasReached == false){
                    qDebug() << "Ziel erreicht mit Abstand" << targetDist << " - Stelle jetzt Winkel perfekt ein";
                }

                positionWasReached = true; // Roboter hat das Ziel erreicht. Wenn das Ziel davonwackelt, ist es immernoch erreicht.

                // Abweichung vom einzustellenden Winkel (kann undefinierte Werte annehmen, falls das Target keinen Winkel besitzt)
                double angleDeviation = fabs(target.getOrientation() - robotPosition.rot());
                if(std::isnan(angleDeviation)) angleDeviation = 0;
                while(angleDeviation > M_PI) angleDeviation -= M_PI; // Die Winkelabweichung darf nie über 180° sein

                if(!targetHasOrientation || (targetHasOrientation && angleDeviation <= config::actorWaypointMaxAngleDeviation ) ) {
                    // Winkel ist OK
                    if(config::enableDebugActorHighLevel)
                        qDebug() << "Ziel erreicht mit Winkelabweichung" << (angleDeviation/M_PI*180.0) << "°";

                    positionWasReached = false; // Zurücksetzen, weil als nächstes ein neues Ziel kommt
                    MapData::deleteFirstTarget(); // Delete this target
                    this->setState(StatePathProcessing::STOP); // Leave the state
                    Q_EMIT signalSendRobotControlParams(0.0, 0.0); // Zur Sicherheit Stop senden
                    break;
                } else {
                    if(config::enableDebugActorHighLevel)
                        qDebug() << "Winkel noch nicht OK - Abweichung " << (angleDeviation/M_PI*180.0) << "°";