C++ CVector2::Length方法代码示例

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

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;
}

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

CVector2 CFootBotFlocking::VectorToLight() {
   /* Get light readings */
   const CCI_FootBotLightSensor::TReadings& tReadings = m_pcLight->GetReadings();
   /* Calculate a normalized vector that points to the closest light */
   CVector2 cAccum;
   for(size_t i = 0; i < tReadings.size(); ++i) {
      cAccum += CVector2(tReadings[i].Value, tReadings[i].Angle);
   }
   if(cAccum.Length() > 0.0f) {
      /* Make the vector long as 1/4 of the max speed */
      cAccum.Normalize();
      cAccum *= 0.25f * m_sWheelTurningParams.MaxSpeed;
   }
   return cAccum;
}

CVector2 CFootBotForaging::CalculateVectorToLight() {
   /* Get readings from light sensor */
   const CCI_FootBotLightSensor::TReadings& tLightReads = m_pcLight->GetReadings();
   /* Sum them together */
   CVector2 cAccumulator;
   for(size_t i = 0; i < tLightReads.size(); ++i) {
      cAccumulator += CVector2(tLightReads[i].Value, tLightReads[i].Angle);
   }
   /* If the light was perceived, return the vector */
   if(cAccumulator.Length() > 0.0f) {
      return CVector2(1.0f, cAccumulator.Angle());
   }
   /* Otherwise, return zero */
   else {
      return CVector2();
   }
}

CVector2 CFootBotForaging::DiffusionVector(bool& b_collision) {
   /* Get readings from proximity sensor */
   const CCI_FootBotProximitySensor::TReadings& tProxReads = m_pcProximity->GetReadings();
   /* Sum them together */
   CVector2 cDiffusionVector;
   for(size_t i = 0; i < tProxReads.size(); ++i) {
      cDiffusionVector += CVector2(tProxReads[i].Value, tProxReads[i].Angle);
   }
   /* If the angle of the vector is small enough and the closest obstacle
      is far enough, ignore the vector and go straight, otherwise return
      it */
   if(m_sDiffusionParams.GoStraightAngleRange.WithinMinBoundIncludedMaxBoundIncluded(cDiffusionVector.Angle()) &&
      cDiffusionVector.Length() < m_sDiffusionParams.Delta ) {
      b_collision = false;
      return CVector2::X;
   }
   else {
      b_collision = true;
      cDiffusionVector.Normalize();
      return -cDiffusionVector;
   }
}

void CBuzzControllerEFootBot::SetWheelSpeedsFromVector(const CVector2& c_heading) {
   /* Get the heading angle */
   CRadians cHeadingAngle = c_heading.Angle().SignedNormalize();
   /* Get the length of the heading vector */
   Real fHeadingLength = c_heading.Length();
   /* Clamp the speed so that it's not greater than MaxSpeed */
   Real fBaseAngularWheelSpeed = Min<Real>(fHeadingLength, m_sWheelTurningParams.MaxSpeed);

   /* Turning state switching conditions */
   if(Abs(cHeadingAngle) <= m_sWheelTurningParams.NoTurnAngleThreshold) {
      /* No Turn, heading angle very small */
      m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::NO_TURN;
   }
   else if(Abs(cHeadingAngle) > m_sWheelTurningParams.HardTurnOnAngleThreshold) {
      /* Hard Turn, heading angle very large */
      m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::HARD_TURN;
   }
   else if(m_sWheelTurningParams.TurningMechanism == SWheelTurningParams::NO_TURN &&
           Abs(cHeadingAngle) > m_sWheelTurningParams.SoftTurnOnAngleThreshold) {
      /* Soft Turn, heading angle in between the two cases */
      m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::SOFT_TURN;
   }

   /* Wheel speeds based on current turning state */
   Real fSpeed1, fSpeed2;
   switch(m_sWheelTurningParams.TurningMechanism) {
      case SWheelTurningParams::NO_TURN: {
         /* Just go straight */
         fSpeed1 = fBaseAngularWheelSpeed;
         fSpeed2 = fBaseAngularWheelSpeed;
         break;
      }

      case SWheelTurningParams::SOFT_TURN: {
         /* Both wheels go straight, but one is faster than the other */
         Real fSpeedFactor = (m_sWheelTurningParams.HardTurnOnAngleThreshold - Abs(cHeadingAngle)) / m_sWheelTurningParams.HardTurnOnAngleThreshold;
         fSpeed1 = fBaseAngularWheelSpeed - fBaseAngularWheelSpeed * (1.0 - fSpeedFactor);
         fSpeed2 = fBaseAngularWheelSpeed + fBaseAngularWheelSpeed * (1.0 - fSpeedFactor);
         break;
      }

      case SWheelTurningParams::HARD_TURN: {
         /* Opposite wheel speeds */
         fSpeed1 = -m_sWheelTurningParams.MaxSpeed;
         fSpeed2 =  m_sWheelTurningParams.MaxSpeed;
         break;
      }
   }

   /* Apply the calculated speeds to the appropriate wheels */
   Real fLeftWheelSpeed, fRightWheelSpeed;
   if(cHeadingAngle > CRadians::ZERO) {
      /* Turn Left */
      fLeftWheelSpeed  = fSpeed1;
      fRightWheelSpeed = fSpeed2;
   }
   else {
      /* Turn Right */
      fLeftWheelSpeed  = fSpeed2;
      fRightWheelSpeed = fSpeed1;
   }
   /* Finally, set the wheel speeds */
   m_pcWheels->SetLinearVelocity(fLeftWheelSpeed, fRightWheelSpeed);
}

void UpdateAgentsFromCrowdData(GSceneManager* crowdEngine, map<UID, IModel*>& agentModels, map<string, float> meshScales, map<string, IMesh*> meshList, map<UID, IModel*>& DestinationVectors, map<UID, IModel*>& MovementVectors)
{
	//TODO: Find correct value for thisScale 

	gen::CMatrix3x3 tempAgentMat;
	float tempModelMat[16];

	for (auto agent : agentModels)
	{
		if (crowdEngine->GetAgentMatrix(agent.first, tempAgentMat))
		{
			float thisScale = meshScales[FindStringFromMesh(agent.second->GetMesh(), meshList)];

			agent.second->GetMatrix(tempModelMat);
			tempModelMat[0] = tempAgentMat.e00 * thisScale;
			
			tempModelMat[2] = tempAgentMat.e01 * thisScale;

			



			tempModelMat[8] = tempAgentMat.e10 * thisScale;
			
			tempModelMat[10] = tempAgentMat.e11 * thisScale;

			tempModelMat[12] = tempAgentMat.e20;
			tempModelMat[14] = tempAgentMat.e21;
			agent.second->SetMatrix(tempModelMat);
		}
	}

#ifdef DirectionVisualiserEnabled
	gen::CVector2 tempDestination;
	for (auto destinationVector : DestinationVectors)
	{
		if (crowdEngine->GetAgentDestination(destinationVector.first, tempDestination))
		{
			IModel* thisAgentModel = agentModels.at(destinationVector.first);

			//Set the vector model to the id model's position
			destinationVector.second->SetPosition(thisAgentModel->GetX(),
				thisAgentModel->GetY() + 10.0f,		//+10 to have to vector model rest atop the id model
				thisAgentModel->GetZ());

			destinationVector.second->LookAt(tempDestination.x, 10.0f, tempDestination.y);
			
			CVector2 vectorToEnd = tempDestination - CVector2(thisAgentModel->GetX(), thisAgentModel->GetZ());
			destinationVector.second->ScaleZ(vectorToEnd.Length());
		}
	}

	for (auto movementVector : MovementVectors)
	{
		//Will only fetch the results of global collision avoidance
		if (crowdEngine->GetAgentDesiredVector(movementVector.first, tempDestination))
		{
			IModel* thisAgentModel = agentModels.at(movementVector.first);

			//Set the vector model to the id model's position
			movementVector.second->SetPosition(thisAgentModel->GetX(),
				thisAgentModel->GetY() + 11.0f,		//+11 to have to vector model rest atop the id model
				thisAgentModel->GetZ());

			movementVector.second->LookAt(tempDestination.x + thisAgentModel->GetX(), 10.0f, tempDestination.y + thisAgentModel->GetZ());

			movementVector.second->ScaleZ(tempDestination.Length());
		}
	}
#endif
}

void UpdateAgentFromCrowdData(UID agentID, GSceneManager* crowdEngine, map<UID, IModel*>& agentModels, map<string, float> meshScales, map<string, IMesh*> meshList, map<UID, IModel*>& DestinationVectors, map<UID, IModel*>& MovementVectors)
{
	gen::CMatrix3x3 tempAgentMat;

	if (crowdEngine->GetAgentMatrix(agentID, tempAgentMat))
	{
		try		//Find the id 'holding' and set the holding skin
		{
			float tempModelMat[16];
			IModel* thisAgentModel;
			thisAgentModel = agentModels.at(agentID);

			float thisScale = meshScales[FindStringFromMesh(thisAgentModel->GetMesh(), meshList)];  

			thisAgentModel->GetMatrix(tempModelMat);
			tempModelMat[0] = tempAgentMat.e00 * thisScale;

			tempModelMat[2] = tempAgentMat.e01 * thisScale;





			tempModelMat[8] = tempAgentMat.e10 * thisScale;

			tempModelMat[10] = tempAgentMat.e11 * thisScale;

			tempModelMat[12] = tempAgentMat.e20;
			tempModelMat[14] = tempAgentMat.e21;
			thisAgentModel->SetMatrix(tempModelMat);
		}
		catch (std::out_of_range err)
		{
			cout << err.what();
		}
	}

#ifdef DirectionVisualiserEnabled
	
	gen::CVector2 tempDestination;
	if (crowdEngine->GetAgentDestination(agentID, tempDestination))
	{
		IModel* thisVectorModel;
		IModel* thisAgentModel;
		thisAgentModel = agentModels.at(agentID);
		thisVectorModel = DestinationVectors.at(agentID);
		
		//Set the vector model to the id model's position
		thisVectorModel->SetPosition(thisAgentModel->GetX(),
		thisAgentModel->GetY() + 10.0f,
		thisAgentModel->GetZ());
		
		thisVectorModel->LookAt(tempDestination.x, 10.0f, tempDestination.y);

		CVector2 vectorToEnd = tempDestination - CVector2(thisAgentModel->GetX(), thisAgentModel->GetZ());
		thisVectorModel->ScaleZ(vectorToEnd.Length());
	}

	if (crowdEngine->GetAgentDesiredVector(agentID, tempDestination))
	{
		IModel* thisVectorModel;
		IModel* thisAgentModel;
		thisAgentModel = agentModels.at(agentID);
		thisVectorModel = MovementVectors.at(agentID);

		//Set the vector model to the id model's position
		thisVectorModel->SetPosition(thisAgentModel->GetX(),
			thisAgentModel->GetY() + 10.0f,
			thisAgentModel->GetZ());

		thisVectorModel->LookAt(tempDestination.x + thisAgentModel->GetX(), 10.0f, tempDestination.y + thisAgentModel->GetZ());

		thisVectorModel->ScaleZ(tempDestination.Length());
	}
#endif
}

bool CBTFootbotRecruiterRootBehavior::IsDoneLookingForFood(){
	CVector2 tmp = m_pcOdometry->GetReversedLocationVector();
	return tmp.Length() <= 7;
}

bool CVector2::operator<=(const CVector2 & a) const
{	
	return Length() <= a.Length();
}

//main render engine function called as multiple threads
void cRenderWorker::doWork(void)
{
	// here will be rendering thread
	QTextStream out(stdout);

	int width = image->GetWidth();
	int height = image->GetHeight();
	double aspectRatio = (double) width / height;

	PrepareMainVectors();
	PrepareReflectionBuffer();
	if (params->ambientOcclusionEnabled && params->ambientOcclusionMode == params::AOmodeMultipeRays) PrepareAOVectors();

	//init of scheduler
	cScheduler *scheduler = threadData->scheduler;

	//start point for ray-marching
	CVector3 start = params->camera;

	scheduler->InitFirstLine(threadData->id, threadData->startLine);

	bool lastLineWasBroken = false;

	//main loop for y
	for (int ys = threadData->startLine; scheduler->ThereIsStillSomethingToDo(threadData->id); ys =
			scheduler->NextLine(threadData->id, ys, lastLineWasBroken))
	{
		//skip if line is out of region
		if (ys < 0) break;
		if (ys < data->screenRegion.y1 || ys > data->screenRegion.y2) continue;

		//main loop for x
		for (int xs = 0; xs < width; xs += scheduler->GetProgressiveStep())
		{

			//break if by coincidence this thread started rendering the same line as some other
			lastLineWasBroken = false;
			if (scheduler->ShouldIBreak(threadData->id, ys))
			{
				lastLineWasBroken = true;
				break;
			}

			if (scheduler->GetProgressivePass() > 1 && xs % (scheduler->GetProgressiveStep() * 2) == 0
					&& ys % (scheduler->GetProgressiveStep() * 2) == 0) continue;

			//skip if pixel is out of region;
			if (xs < data->screenRegion.x1 || xs > data->screenRegion.x2) continue;

			//calculate point in image coordinate system
			CVector2<int> screenPoint(xs, ys);
			CVector2<double> imagePoint = data->screenRegion.transpose(data->imageRegion, screenPoint);
			imagePoint.x *= aspectRatio;

			//full dome shemisphere cut
			bool hemisphereCut = false;
			if (params->perspectiveType == params::perspFishEyeCut
					&& imagePoint.Length() > 0.5 / params->fov) hemisphereCut = true;

			//calculate direction of ray-marching
			CVector3 viewVector = CalculateViewVector(imagePoint,
																								params->fov,
																								params->perspectiveType,
																								mRot);

			//---------------- 1us -------------

			//Ray marching
			CVector3 point;
			CVector3 startRay = start;
			sRGBAfloat resultShader;
			sRGBAfloat objectColour;
			CVector3 normal;
			double depth = 1e20;
			double opacity = 1.0;

			//raymarching loop (reflections)

			if (!hemisphereCut) //in fulldome mode, will not render pixels out of the fulldome
			{
				sRayRecursionIn recursionIn;

				sRayMarchingIn rayMarchingIn;
				CVector3 direction = viewVector;
				direction.Normalize();
				rayMarchingIn.binaryEnable = true;
				rayMarchingIn.direction = direction;
				rayMarchingIn.maxScan = params->viewDistanceMax;
				rayMarchingIn.minScan = params->viewDistanceMin;
				rayMarchingIn.start = startRay;
				rayMarchingIn.invertMode = false;
				recursionIn.rayMarchingIn = rayMarchingIn;
				recursionIn.calcInside = false;
				recursionIn.resultShader = resultShader;
				recursionIn.objectColour = objectColour;

				sRayRecursionInOut recursionInOut;
				sRayMarchingInOut rayMarchingInOut;
				rayMarchingInOut.buffCount = &rayBuffer[0].buffCount;
//.........这里部分代码省略.........