C++ CVector2类代码示例

bool Mbfo::isRoadClear(const CVector2& obstacleProximity) {
    CDegrees obstacleAngle(ToDegrees(obstacleProximity.Angle()));
    CDegrees safeAngle(150.0f);
    return (minAngleFromObstacle.WithinMinBoundIncludedMaxBoundIncluded(obstacleAngle)
            && obstacleProximity.Length() < minDistanceFromObstacle) ||
            (obstacleAngle < -safeAngle || obstacleAngle > safeAngle);
}

void CFootBotUN::addNodesAsObstacles(map<UInt8, Node> listNodeObstacles) {

	//Aux variables
	CVector2 auxPosition;
	CVector2 auxVelocity;

	//Create a list of Nodes
	for (map<UInt8, Node>::iterator it = listNodeObstacles.begin(); it != listNodeObstacles.end(); it++) {

		//printf("Node %d\n", (it->second).getId());

		// I'm not and obstacle for myself!
		if ((it->second).getId() != mySelf.getId()) {

			//printf("Agent \n");
			auxPosition.Set((it->second).getPosition().GetX(), (it->second).getPosition().GetY());
			auxVelocity.Set((it->second).getVelocity(), 0);

			// For a dynamic obstacle we need to use the addObstacleAtPoint(position,velocity,radius)
			//agent->addObstacleAtPoint(auxPosition, OBSTACLE_RADIUS);
			agent->addObstacleAtPoint(auxPosition, auxVelocity, OBSTACLE_RADIUS);

			printf("Adding obstacle : %d\n", (it->second).getId());
		}

	}

}

   void CFootBotMotorGroundSensor::Update() {
      /* We make the assumption that the foot-bot is rotated only wrt to Z */
      CFloorEntity& cFloorEntity = m_cSpace.GetFloorEntity();
      const CVector3& cEntityPos = GetEntity().GetEmbodiedEntity().GetPosition();
      const CQuaternion& cEntityRot = GetEntity().GetEmbodiedEntity().GetOrientation();
      CRadians cRotZ, cRotY, cRotX;
      cEntityRot.ToEulerAngles( cRotZ, cRotY, cRotX );
      CVector2 cCenterPos(cEntityPos.GetX(), cEntityPos.GetY());
      CVector2 cSensorPos;
      for(UInt32 i = 0; i < CCI_FootBotMotorGroundSensor::NUM_READINGS; ++i) {
         cSensorPos  = m_tReadings[i].Offset;
         cSensorPos.Rotate(cRotZ);
	 cSensorPos *= 0.01;
	 cSensorPos += cCenterPos;
         const CColor& cColor = cFloorEntity.GetColorAtPoint(cSensorPos.GetX(),cSensorPos.GetY());
         m_tReadings[i].Value = cColor.ToGrayScale()/255*FOOTBOT_MOTOR_GROUND_SENSOR_READING_RANGE.GetSpan();

	 if( m_fNoiseLevel > 0.0f ) {
	    AddNoise(i);
	 }

	 /* Normalize reading between 0 and 1, only if calibration has been performed */
         if( m_bCalibrated ) {
	    m_tReadings[i].Value = FOOTBOT_MOTOR_GROUND_SENSOR_READING_RANGE.NormalizeValue(m_tReadings[i].Value);
	 }
      }
   }

 void CFootBotBaseGroundRotZOnlySensor::Update() {
    /*
     * We make the assumption that the robot is rotated only wrt to Z
     */
    /* Get robot position and orientation */
    const CVector3& cEntityPos = m_pcEmbodiedEntity->GetPosition();
    const CQuaternion& cEntityRot = m_pcEmbodiedEntity->GetOrientation();
    CRadians cRotZ, cRotY, cRotX;
    cEntityRot.ToEulerAngles(cRotZ, cRotY, cRotX);
    /* Set robot center */
    CVector2 cCenterPos(cEntityPos.GetX(), cEntityPos.GetY());
    /* Position of sensor on the ground after rototranslation */
    CVector2 cSensorPos;
    /* Go through the sensors */
    for(UInt32 i = 0; i < m_tReadings.size(); ++i) {
       /* Calculate sensor position on the ground */
       cSensorPos = m_pcGroundSensorEntity->GetSensor(i+4).Offset;
       cSensorPos.Rotate(cRotZ);
       cSensorPos += cCenterPos;
       /* Get the color */
       const CColor& cColor = m_pcFloorEntity->GetColorAtPoint(cSensorPos.GetX(),
                                                               cSensorPos.GetY());
       /* Set the reading */
       m_tReadings[i].Value = cColor.ToGrayScale() / 255.0f;
       /* Apply noise to the sensor */
       if(m_bAddNoise) {
          m_tReadings[i].Value += m_pcRNG->Uniform(m_cNoiseRange);
       }
       /* Set the final reading */
       m_tReadings[i].Value = m_tReadings[i].Value < 0.5f ? 0.0f : 1.0f;
    }
 }

  bool CKinematics2DCollisionCircle::CheckIntersectionWithRay(Real& f_distance, const CRay& c_ray) const {
    CSegment c_segment = c_ray.ProjectOntoXY();
    CVector2 c_closest_point, c_closest_segment_point;
    c_segment.GetMinimumDistancePoints( m_cPosition, c_closest_point, c_closest_segment_point );
    
    Real f_min_segment_distance = Distance(m_cPosition,c_closest_segment_point);
    if( f_min_segment_distance > m_fRadius ) {
      return false;
    }

    // see http://local.wasp.uwa.edu.au/~pbourke/geometry/sphereline/
    CVector2 dp = c_segment.GetEnd() - c_segment.GetStart();
    Real     a  = dp.SquareLength();
    Real     b  = 2 * dp.DotProduct(c_segment.GetStart() - m_cPosition);
    Real     c  = (m_cPosition.SquareLength() +
		   c_segment.GetStart().SquareLength() -
		   2 * m_cPosition.DotProduct(c_segment.GetStart()) -
		   m_fRadius*m_fRadius);
    Real bb4ac  = b*b - 4*a*c;
    Real mu1    = (-b + sqrt(bb4ac)) / (2 * a);
    Real mu2    = (-b - sqrt(bb4ac)) / (2 * a);
    
    Real mu = Min(mu1,mu2);
    if( mu < 0 || mu > 1 ) mu = Max(mu1,mu2);

    f_distance = mu * c_ray.GetLength();    
    return true;
  }

void CBTFootbotRecruiterRootBehavior::GoToFood(){

	CVector2 tmp = m_pcObstacleAvoidance->GetVector();
	tmp += m_pcOdometry->GetReversedLocationVector();
	tmp.Normalize();
	GoToVector(tmp);
	m_pcOdometry->Step(*c_robot_state);
}

/**
 * Returns true if the given vector is near the arena end.
 */
bool CCombinedLoopFunctions::CloseToArenaEnd(const CVector2& pos){

	if(pos.GetX() > arenaSize.GetX()/2.0f - CLOSE_TO_ARENA_END_PARAMETER ||
			pos.GetX() < -arenaSize.GetX()/2.0f + CLOSE_TO_ARENA_END_PARAMETER ||
			pos.GetY() > arenaSize.GetY()/2.0f - CLOSE_TO_ARENA_END_PARAMETER ||
			pos.GetY() < -arenaSize.GetY()/2.0f + CLOSE_TO_ARENA_END_PARAMETER) {
		return true;
	}
	return false;
}

/**
 * Returns true if the given vector is near the nest, relative to food patch size.
 */
bool CCombinedLoopFunctions::CloseToNest(const CVector2& pos){
	/*if(pos.GetX() > -(nestSize/2.0f)-foodPatchSize/1 && pos.GetX() < (nestSize/2.0f) + foodPatchSize/1 && pos.GetY() > -(nestSize/2.0f) - foodPatchSize/1 && pos.GetY() < (nestSize/2.0f) + foodPatchSize/1) {
		return true;
	}
	return false;*/

	if(pos.GetX() > -(nestSize/2.0f) - CLOSE_TO_NEST_PARAMETER && pos.GetX() < (nestSize/2.0f) + CLOSE_TO_NEST_PARAMETER && pos.GetY() > -(nestSize/2.0f) - CLOSE_TO_NEST_PARAMETER && pos.GetY() < (nestSize/2.0f) + CLOSE_TO_NEST_PARAMETER) {
		return true;
	}
	return false;
}

CVector2 Agent::relativePositionOfObstacleAt(CVector2 &obstaclePosition, Real obstacleRadius, Real &distance) {
	CVector2 relativePosition = obstaclePosition - position;
	distance = relativePosition.Length();
	Real minDistance = (radius + obstacleRadius) + 0.002;
	if (distance < minDistance) {
		//too near,  cannot penetrate in an obstacle (footbot or human)
		relativePosition = relativePosition / distance * minDistance;
		obstaclePosition = position + relativePosition;
		distance = minDistance;
	}
	return relativePosition;
}

void CCameraData::ProjectDepthRay( const CVector2& v2d, CVector3& vdir, CVector3& vori ) const
{
	const Frustum& camfrus = mFrustum;
	CVector3 near_xt_lerp; near_xt_lerp.Lerp( camfrus.mNearCorners[0], camfrus.mNearCorners[1], v2d.GetX() );
	CVector3 near_xb_lerp; near_xb_lerp.Lerp( camfrus.mNearCorners[3], camfrus.mNearCorners[2], v2d.GetX() );
	CVector3 near_lerp; near_lerp.Lerp( near_xt_lerp, near_xb_lerp, v2d.GetY() );
	CVector3 far_xt_lerp; far_xt_lerp.Lerp( camfrus.mFarCorners[0], camfrus.mFarCorners[1], v2d.GetX() );
	CVector3 far_xb_lerp; far_xb_lerp.Lerp( camfrus.mFarCorners[3], camfrus.mFarCorners[2], v2d.GetX() );
	CVector3 far_lerp; far_lerp.Lerp( far_xt_lerp, far_xb_lerp, v2d.GetY() );
	vdir=(far_lerp-near_lerp).Normal();
	vori=near_lerp;
}

void CFootBotUN::updateDesideredVelocity() {

	CVector2 agentToTarget = targetPosition - position;

	CRadians a0 = agentToTarget.Angle() - angle;

	DEBUG_CONTROLLER("updateDesideredVelocity to %.2f, state = %d \r\n",a0.GetValue(),state);

	agent->targetPosition = targetPosition;
	agent->updateDesideredVelocity();

	printf("=> desidered speed %.2f, desidered angle %.2f \n", agent->desideredSpeed, agent->desideredAngle.GetValue());
}

bool iAnt_controller::checkDistanceY() {
    cout << "checkDistanceY\n";
    CVector2 curPosition = GetPosition();
    cout << "curPositionY" << curPosition << '\n';
    bool stop = false;
    //cout << "curPosition" << curPosition.GetY() << '\n';
    float distanceY = curPosition.GetX()- data->NestPosition.GetX();
    cout << "distance: " << distanceY << "\n";
    if (distanceY > 0.3)
    {   cout << "line 115";
        stop = true;
    }
    return stop;
}

void CCameraData::GetPixelLengthVectors( const CVector3& Pos, const CVector2& vp, CVector3& OutX, CVector3& OutY ) const
{
	/////////////////////////////////////////////////////////////////
	int ivpw = int(vp.GetX());
	int ivph = int(vp.GetY());
	/////////////////////////////////////////////////////////////////
	CVector4 va = Pos;
	CVector4 va_xf = va.Transform(mVPMatrix);
	va_xf.PerspectiveDivide();
	va_xf = va_xf*CVector4(vp.GetX(),vp.GetY(),0.0f);
	/////////////////////////////////////////////////////////////////
	CVector4 vdx = Pos+mCamXNormal;
	CVector4 vdx_xf = vdx.Transform(mVPMatrix);
	vdx_xf.PerspectiveDivide();
	vdx_xf = vdx_xf*CVector4(vp.GetX(),vp.GetY(),0.0f);
	float MagX = (vdx_xf-va_xf).Mag(); // magnitude in pixels of mBillboardRight
	/////////////////////////////////////////////////////////////////
	CVector4 vdy = Pos+mCamYNormal;
	CVector4 vdy_xf = vdy.Transform(mVPMatrix);
	vdy_xf.PerspectiveDivide();
	vdy_xf = vdy_xf*CVector4(vp.GetX(),vp.GetY(),0.0f);
	float MagY = (vdy_xf-va_xf).Mag(); // magnitude in pixels of mBillboardUp
	/////////////////////////////////////////////////////////////////
	OutX = mCamXNormal*(2.0f/MagX);
	OutY = mCamYNormal*(2.0f/MagY);
	/////////////////////////////////////////////////////////////////
}

CColor CLandmarks::GetFloorColor(const CVector2& c_position_on_plane) {
   if((c_position_on_plane.GetX() >= -2.0f &&
       c_position_on_plane.GetX() <= 2.0f) &&
      (c_position_on_plane.GetY() >= -5.0f &&
       c_position_on_plane.GetY() <= -1.0f)) {
      return CColor::GRAY90;
   }
   for(size_t i = 0; i < TARGETS; ++i) {
      if(SquareDistance(c_position_on_plane, m_vecTargets[i]) < TARGET_RADIUS2) {
         return CColor::BLACK;
      }
   }
   return CColor::WHITE;
}

CVector2 CFootBotFlocking::FlockingVector() {
   /* Get the camera readings */
   const CCI_ColoredBlobOmnidirectionalCameraSensor::SReadings& sReadings = m_pcCamera->GetReadings();
   /* Go through the camera readings to calculate the flocking interaction vector */
   if(! sReadings.BlobList.empty()) {
      CVector2 cAccum;
      Real fLJ;
      size_t unBlobsSeen = 0;

      for(size_t i = 0; i < sReadings.BlobList.size(); ++i) {

         /*
          * The camera perceives the light as a yellow blob
          * The robots have their red beacon on
          * So, consider only red blobs
          * In addition: consider only the closest neighbors, to avoid
          * attraction to the farthest ones. Taking 180% of the target
          * distance is a good rule of thumb.
          */
         if(sReadings.BlobList[i]->Color == CColor::RED &&
            sReadings.BlobList[i]->Distance < m_sFlockingParams.TargetDistance * 1.80f) {
            /*
             * Take the blob distance and angle
             * With the distance, calculate the Lennard-Jones interaction force
             * Form a 2D vector with the interaction force and the angle
             * Sum such vector to the accumulator
             */
            /* Calculate LJ */
            fLJ = m_sFlockingParams.GeneralizedLennardJones(sReadings.BlobList[i]->Distance);
            /* Sum to accumulator */
            cAccum += CVector2(fLJ,
                               sReadings.BlobList[i]->Angle);
            /* Increment the blobs seen counter */
            ++unBlobsSeen;
         }
      }
      /* Divide the accumulator by the number of blobs seen */
      cAccum /= unBlobsSeen;
      /* Clamp the length of the vector to the max speed */
      if(cAccum.Length() > m_sWheelTurningParams.MaxSpeed) {
         cAccum.Normalize();
         cAccum *= m_sWheelTurningParams.MaxSpeed;
      }
      return cAccum;
   }
   else {
      return CVector2();
   }
}