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(); }
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; rayMarchingInOut.stepBuff = rayBuffer[0].stepBuff; recursionInOut.rayMarchingInOut = rayMarchingInOut; recursionInOut.rayIndex = 0; sRayRecursionOut recursionOut = RayRecursion(recursionIn, recursionInOut); resultShader = recursionOut.resultShader; objectColour = recursionOut.objectColour; depth = recursionOut.rayMarchingOut.depth; if (!recursionOut.found) depth = 1e20; opacity = recursionOut.fogOpacity; normal = recursionOut.normal; } sRGBfloat pixel2; pixel2.R = resultShader.R; pixel2.G = resultShader.G; pixel2.B = resultShader.B; unsigned short alpha = resultShader.A * 65535; unsigned short opacity16 = opacity * 65535; sRGB8 colour; colour.R = objectColour.R * 255; colour.G = objectColour.G * 255; colour.B = objectColour.B * 255; sRGBfloat normalFloat; if(image->GetImageOptional()->optionalNormal) { CVector3 normalRotated = mRotInv.RotateVector(normal); normalFloat.R = (1.0 + normalRotated.x) / 2.0; normalFloat.G = (1.0 + normalRotated.z) / 2.0; normalFloat.B = 1.0 - normalRotated.y; } for (int xx = 0; xx < scheduler->GetProgressiveStep(); ++xx) { for (int yy = 0; yy < scheduler->GetProgressiveStep(); ++yy) { image->PutPixelImage(screenPoint.x + xx, screenPoint.y + yy, pixel2); image->PutPixelColour(screenPoint.x + xx, screenPoint.y + yy, colour); image->PutPixelAlpha(screenPoint.x + xx, screenPoint.y + yy, alpha); image->PutPixelZBuffer(screenPoint.x + xx, screenPoint.y + yy, (float) depth); image->PutPixelOpacity(screenPoint.x + xx, screenPoint.y + yy, opacity16); if(image->GetImageOptional()->optionalNormal) image->PutPixelNormal(screenPoint.x + xx, screenPoint.y + yy, normalFloat); } } data->statistics.numberOfRenderedPixels++; } } //emit signal to main thread when finished emit finished(); return; }