CRay3 VoronoiDiagram::ToVoronoiEdge(const Edge& edge) const { CRay3 voronoiEdge; if (edge.is_finite()) { voronoiEdge.SetStart(ToVector3(*edge.vertex0())); voronoiEdge.SetEnd(ToVector3(*edge.vertex1())); } else { const auto& cell1 = *edge.cell(); const auto& cell2 = *edge.twin()->cell(); VoronoiDiagram::Point origin, direction; VoronoiDiagram::Point p1 = boostPoints.at(cell1.source_index()); VoronoiDiagram::Point p2 = boostPoints.at(cell2.source_index()); p1.set(HORIZONTAL, p1.x()/scaleVectorToMilimeters); p1.set(VERTICAL, p1.y()/scaleVectorToMilimeters); p2.set(HORIZONTAL, p2.x()/scaleVectorToMilimeters); p2.set(VERTICAL, p2.y()/scaleVectorToMilimeters); origin.x((p1.x() + p2.x()) * 0.5); origin.y((p1.y() + p2.y()) * 0.5); direction.x(p1.y() - p2.y()); direction.y(p2.x() - p1.x()); Real side = arenaLimits.GetMax().GetX()*2; Real koef = side / max(fabs(direction.x()), fabs(direction.y())); if (edge.vertex0() == NULL) { CVector3 start; start.SetX(origin.x() - (direction.x() * koef)); start.SetY(origin.y() - (direction.y() * koef)); start.SetZ(diagramLiftOnZ); voronoiEdge.SetStart(start); } else { voronoiEdge.SetStart(ToVector3(*edge.vertex0())); } if (edge.vertex1() == NULL) { CVector3 end; end.SetX(origin.x() + direction.x() * koef); end.SetY(origin.y() + direction.y() * koef); end.SetZ(diagramLiftOnZ); voronoiEdge.SetEnd(end); } else { voronoiEdge.SetEnd(ToVector3(*edge.vertex1())); } } return voronoiEdge; }
void CFootBotLightRotZOnlySensor::Update() { /* Erase readings */ for(size_t i = 0; i < m_tReadings.size(); ++i) { m_tReadings[i].Value = 0.0f; } /* Get foot-bot orientation */ CRadians cTmp1, cTmp2, cOrientationZ; m_pcEmbodiedEntity->GetOriginAnchor().Orientation.ToEulerAngles(cOrientationZ, cTmp1, cTmp2); /* Ray used for scanning the environment for obstacles */ CRay3 cOcclusionCheckRay; cOcclusionCheckRay.SetStart(m_pcEmbodiedEntity->GetOriginAnchor().Position); CVector3 cRobotToLight; /* Buffer for the angle of the light wrt to the foot-bot */ CRadians cAngleLightWrtFootbot; /* Buffers to contain data about the intersection */ SEmbodiedEntityIntersectionItem sIntersection; /* List of light entities */ CSpace::TMapPerTypePerId::iterator itLights = m_cSpace.GetEntityMapPerTypePerId().find("light"); if (itLights != m_cSpace.GetEntityMapPerTypePerId().end()) { CSpace::TMapPerType& mapLights = itLights->second; /* * 1. go through the list of light entities in the scene * 2. check if a light is occluded * 3. if it isn't, distribute the reading across the sensors * NOTE: the readings are additive * 4. go through the sensors and clamp their values */ for(CSpace::TMapPerType::iterator it = mapLights.begin(); it != mapLights.end(); ++it) { /* Get a reference to the light */ CLightEntity& cLight = *(any_cast<CLightEntity*>(it->second)); /* Consider the light only if it has non zero intensity */ if(cLight.GetIntensity() > 0.0f) { /* Set the ray end */ cOcclusionCheckRay.SetEnd(cLight.GetPosition()); /* Check occlusion between the foot-bot and the light */ if(! GetClosestEmbodiedEntityIntersectedByRay(sIntersection, cOcclusionCheckRay, *m_pcEmbodiedEntity)) { /* The light is not occluded */ if(m_bShowRays) { m_pcControllableEntity->AddCheckedRay(false, cOcclusionCheckRay); } /* Get the distance between the light and the foot-bot */ cOcclusionCheckRay.ToVector(cRobotToLight); /* * Linearly scale the distance with the light intensity * The greater the intensity, the smaller the distance */ cRobotToLight /= cLight.GetIntensity(); /* Get the angle wrt to foot-bot rotation */ cAngleLightWrtFootbot = cRobotToLight.GetZAngle(); cAngleLightWrtFootbot -= cOrientationZ; /* * Find closest sensor index to point at which ray hits footbot body * Rotate whole body by half a sensor spacing (corresponding to placement of first sensor) * Division says how many sensor spacings there are between first sensor and point at which ray hits footbot body * Increase magnitude of result of division to ensure correct rounding */ Real fIdx = (cAngleLightWrtFootbot - SENSOR_HALF_SPACING) / SENSOR_SPACING; SInt32 nReadingIdx = (fIdx > 0) ? fIdx + 0.5f : fIdx - 0.5f; /* Set the actual readings */ Real fReading = cRobotToLight.Length(); /* * Take 6 readings before closest sensor and 6 readings after - thus we * process sensors that are with 180 degrees of intersection of light * ray with robot body */ for(SInt32 nIndexOffset = -6; nIndexOffset < 7; ++nIndexOffset) { UInt32 unIdx = Modulo(nReadingIdx + nIndexOffset, 24); CRadians cAngularDistanceFromOptimalLightReceptionPoint = Abs((cAngleLightWrtFootbot - m_tReadings[unIdx].Angle).SignedNormalize()); /* * ComputeReading gives value as if sensor was perfectly in line with * light ray. We then linearly decrease actual reading from 1 (dist * 0) to 0 (dist PI/2) */ m_tReadings[unIdx].Value += ComputeReading(fReading) * ScaleReading(cAngularDistanceFromOptimalLightReceptionPoint); } } else { /* The ray is occluded */ if(m_bShowRays) { m_pcControllableEntity->AddCheckedRay(true, cOcclusionCheckRay); m_pcControllableEntity->AddIntersectionPoint(cOcclusionCheckRay, sIntersection.TOnRay); } } } } /* Apply noise to the sensors */ if(m_bAddNoise) { for(size_t i = 0; i < 24; ++i) { m_tReadings[i].Value += m_pcRNG->Uniform(m_cNoiseRange); } } /* Trunc the reading between 0 and 1 */ for(size_t i = 0; i < 24; ++i) { SENSOR_RANGE.TruncValue(m_tReadings[i].Value); } } else { /* There are no lights in the environment */ if(m_bAddNoise) { /* Go through the sensors */ for(UInt32 i = 0; i < m_tReadings.size(); ++i) { /* Apply noise to the sensor */ m_tReadings[i].Value += m_pcRNG->Uniform(m_cNoiseRange); /* Trunc the reading between 0 and 1 */ SENSOR_RANGE.TruncValue(m_tReadings[i].Value); } } } }
void CRABMedium::Update() { /* Update positional index of RAB entities */ m_pcRABEquippedEntityIndex->Update(); /* Delete routing table */ for(TRoutingTable::iterator it = m_tRoutingTable.begin(); it != m_tRoutingTable.end(); ++it) { it->second.clear(); } /* This map contains the pairs that have already been checked */ unordered_map<ssize_t, std::pair<CRABEquippedEntity*, CRABEquippedEntity*> > mapPairsAlreadyChecked; /* Iterator for the above structure */ unordered_map<ssize_t, std::pair<CRABEquippedEntity*, CRABEquippedEntity*> >::iterator itPair; /* Used as test key */ std::pair<CRABEquippedEntity*, CRABEquippedEntity*> cTestKey; /* Used as hash for the test key */ UInt64 unTestHash; /* The ray to use for occlusion checking */ CRay3 cOcclusionCheckRay; /* Buffer for the communicating entities */ CSet<CRABEquippedEntity*,SEntityComparator> cOtherRABs; /* Buffer to store the intersection data */ SEmbodiedEntityIntersectionItem sIntersectionItem; /* The distance between two RABs in line of sight */ Real fDistance; /* Go through the RAB entities */ for(TRoutingTable::iterator it = m_tRoutingTable.begin(); it != m_tRoutingTable.end(); ++it) { /* Get a reference to the current RAB entity */ CRABEquippedEntity& cRAB = *reinterpret_cast<CRABEquippedEntity*>(GetSpace().GetEntityVector()[it->first]); /* Initialize the occlusion check ray start to the position of the robot */ cOcclusionCheckRay.SetStart(cRAB.GetPosition()); /* For each RAB entity, get the list of RAB entities in range */ cOtherRABs.clear(); m_pcRABEquippedEntityIndex->GetEntitiesAt(cOtherRABs, cRAB.GetPosition()); /* Go through the RAB entities in range */ for(CSet<CRABEquippedEntity*>::iterator it2 = cOtherRABs.begin(); it2 != cOtherRABs.end(); ++it2) { /* Get a reference to the RAB entity */ CRABEquippedEntity& cOtherRAB = **it2; /* First, make sure the entities are not the same */ if(&cRAB != &cOtherRAB) { /* Proceed if the pair has not been checked already */ if(&cRAB < &cOtherRAB) { cTestKey.first = &cRAB; cTestKey.second = &cOtherRAB; } else { cTestKey.first = &cOtherRAB; cTestKey.second = &cRAB; } unTestHash = HashRABPair(cTestKey); itPair = mapPairsAlreadyChecked.find(unTestHash); if(itPair == mapPairsAlreadyChecked.end() || /* Pair does not exist */ itPair->second.first != cTestKey.first || /* Pair exists, but first RAB involved is different */ itPair->second.second != cTestKey.second) { /* Pair exists, but second RAB involved is different */ /* Mark this pair as already checked */ mapPairsAlreadyChecked[unTestHash] = cTestKey; /* Proceed if the message size is compatible */ if(cRAB.GetMsgSize() == cOtherRAB.GetMsgSize()) { /* Proceed if the two entities are not obstructed by another object */ cOcclusionCheckRay.SetEnd(cOtherRAB.GetPosition()); if((!m_bCheckOcclusions) || (!GetClosestEmbodiedEntityIntersectedByRay(sIntersectionItem, cOcclusionCheckRay, cRAB.GetEntityBody())) || (&cOtherRAB.GetEntityBody() == sIntersectionItem.IntersectedEntity)) { /* If we get here, the two RAB entities are in direct line of sight */ /* cRAB can receive cOtherRAB's message if it is in range, and viceversa */ /* Calculate square distance */ fDistance = cOcclusionCheckRay.GetLength(); if(fDistance < cOtherRAB.GetRange()) { /* cRAB receives cOtherRAB's message */ m_tRoutingTable[cRAB.GetIndex()].insert(&cOtherRAB); } if(fDistance < cRAB.GetRange()) { /* cOtherRAB receives cRAB's message */ m_tRoutingTable[cOtherRAB.GetIndex()].insert(&cRAB); } } // occlusion found? } // is msg size the same? } // is check necessary? } // are entities the same? } // for entities in range } // for routing table }