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