bool CPlane::Intersects(Real& f_t_on_ray,
const CRay3& c_ray) {
/* Ray direction */
CVector3 cRayDir;
c_ray.GetDirection(cRayDir);
/* Calculate f_t_on_ray */
Real fNumerator = (m_cPosition-c_ray.GetStart()).DotProduct(m_cNormal);
Real fDenominator = cRayDir.DotProduct(m_cNormal);
/* Is ray parallel to plane? */
if(Abs(fDenominator) > 1e-6) {
/* No, it's not */
f_t_on_ray = fNumerator / fDenominator / c_ray.GetLength();
return (f_t_on_ray < 1.0f);
}
else {
/* Yes, it is */
/* Is ray coincident with the plane? */
if(Abs(fNumerator) > 1e-6) {
/* No, the ray is parallel to and far from the plane */
/* No intersection possible */
return false;
}
else {
/* Yes, the ray coincides with the plane */
f_t_on_ray = 0.0f;
return true;
}
}
}
Real CFootBotDistanceScannerRotZOnlySensor::CalculateReadingForRay(const CRay3& c_ray,
Real f_min_distance) {
/* Get the closest intersection */
SEmbodiedEntityIntersectionItem sIntersection;
if(GetClosestEmbodiedEntityIntersectedByRay(sIntersection,
c_ray,
*m_pcEmbodiedEntity)) {
if(m_bShowRays) m_pcControllableEntity->AddIntersectionPoint(c_ray, sIntersection.TOnRay);
/* There is an intersection! */
Real fDistance = c_ray.GetDistance(sIntersection.TOnRay);
if(fDistance > f_min_distance) {
/* The distance is returned in meters, but the reading must be in cm */
if(m_bShowRays) m_pcControllableEntity->AddCheckedRay(true, c_ray);
return fDistance * 100.0f;
}
else {
/* The detected intersection was too close */
if(m_bShowRays) m_pcControllableEntity->AddCheckedRay(true, c_ray);
return -1.0f;
}
}
else {
/* No intersection */
if(m_bShowRays) m_pcControllableEntity->AddCheckedRay(false, c_ray);
return -2.0f;
}
}
CRay3 VoronoiDiagram::getRayBoundedToArena(const CRay3 &ray) const {
Real startX, startY, endX, endY;
bool edgeWasClipped = LiangBarsky(
arenaLimits.GetMin().GetX(),
arenaLimits.GetMax().GetX(),
arenaLimits.GetMin().GetY(),
arenaLimits.GetMax().GetY(),
ray.GetStart().GetX(), ray.GetStart().GetY(),
ray.GetEnd().GetX(), ray.GetEnd().GetY(),
startX, startY, endX, endY);
if (!edgeWasClipped)
throw EdgeNotInArea();
CRay3 boundedRay(CVector3(startX, startY, diagramLiftOnZ),
CVector3(endX, endY, diagramLiftOnZ));\
return boundedRay;
}
/**
* Check for ray intersection
*/
bool CBulletSphereModel::CheckIntersectionWithRay(Real &f_t_on_ray, const CRay3 &ray) const
{
CVector3 rayOrigin = ray.GetStart();
CVector3 rayDirection;
ray.GetDirection(rayDirection);
CVector3 sourceToOrigin = rayOrigin - position;
double sourceToOriginLength = sourceToOrigin.Length();
double lineDotSourceToOrigin = rayDirection.DotProduct(sourceToOrigin);
double solutionCheck = pow(lineDotSourceToOrigin, 2);
solutionCheck -= pow(sourceToOriginLength, 2);
solutionCheck += pow(entity->GetRadius(), 2);
if(solutionCheck < 0)
return false;
f_t_on_ray = -lineDotSourceToOrigin - sqrt(solutionCheck);
return true;
}
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 CQTOpenGLUserFunctions::DrawRay(const CRay3& c_ray,
const CColor& c_color,
Real f_width) {
/* Save attributes and current matrix */
glPushAttrib(GL_LINE_BIT);
/* Set line attributes */
glEnable(GL_LINE_SMOOTH);
glLineWidth(f_width);
/* Set color */
SetColor(c_color);
/* Draw ray */
glBegin(GL_LINES);
glVertex3f(c_ray.GetStart().GetX(), c_ray.GetStart().GetY(), c_ray.GetStart().GetZ());
glVertex3f(c_ray.GetEnd().GetX(), c_ray.GetEnd().GetY(), c_ray.GetEnd().GetZ());
glEnd();
/* Restore saved stuff */
glPopAttrib();
}
bool CCylinder::Intersects(Real& f_t_on_ray,
const CRay3& c_ray) {
/*
* This algorithm was adapted from
* http://www.realtimerendering.com/resources/GraphicsGems/gemsiv/ray_cyl.c
*/
/* Vector from cylinder base to ray start */
CVector3 cCylBase2RayStart(c_ray.GetStart());
cCylBase2RayStart -= m_cBasePos;
/* Ray direction and length */
CVector3 cRayDir;
c_ray.GetDirection(cRayDir);
Real fRayLen = c_ray.GetLength();
/* Vector normal to cylinder axis and ray direction */
CVector3 cNormal(cRayDir);
cNormal.CrossProduct(m_cAxis);
Real fNormalLen = cNormal.Length();
/* Are cylinder axis and ray parallel? */
if(fNormalLen > 0) {
/* No, they aren't parallel */
/* Make normal have length 1 */
cNormal /= fNormalLen;
/* Calculate shortest distance between axis and ray
* by projecting cCylBase2RayStart onto cNormal */
Real fDist = Abs(cCylBase2RayStart.DotProduct(cNormal));
/* Is fDist smaller than the cylinder radius? */
if(fDist > m_fRadius) {
/* No, it's not, so there can't be any intersection */
return false;
}
/* If we get here, it's because the ray intersects the infinite cylinder */
/* Create a buffer for the 4 potential intersection points
(two on the sides, two on the bases) */
Real fPotentialT[4];
/* First, calculate the intersection points with the sides */
/* Calculate the midpoint between the two intersection points */
CVector3 cVec(cCylBase2RayStart);
cVec.CrossProduct(m_cAxis);
Real fMidPointDist = -cVec.DotProduct(cNormal) / fNormalLen;
/* Calculate the distance between the midpoint and the potential t's */
cVec = cNormal;
cVec.CrossProduct(m_cAxis);
cVec.Normalize();
Real fDeltaToMidPoint = Abs(Sqrt(Square(m_fRadius) - Square(fDist)) / cRayDir.DotProduct(cVec));
/* Calculate the potential t's on the infinite surface */
fPotentialT[0] = (fMidPointDist - fDeltaToMidPoint) / fRayLen;
fPotentialT[1] = (fMidPointDist + fDeltaToMidPoint) / fRayLen;
/* Make sure these t's correspond to points within the cylinder bases */
CVector3 cPoint;
c_ray.GetPoint(cPoint, fPotentialT[0]);
if((cPoint - m_cBasePos).DotProduct(m_cAxis) < 0 ||
(cPoint - (m_cBasePos + m_fHeight * m_cAxis)).DotProduct(m_cAxis) > 0) {
fPotentialT[0] = -1;
}
c_ray.GetPoint(cPoint, fPotentialT[1]);
if((cPoint - m_cBasePos).DotProduct(m_cAxis) < 0 ||
(cPoint - (m_cBasePos + m_fHeight * m_cAxis)).DotProduct(m_cAxis) > 0) {
fPotentialT[1] = -1;
}
/* Check whether the ray is contained within the cylinder bases */
Real fDenominator = cRayDir.DotProduct(m_cAxis);
/* Is ray parallel to plane? */
if(Abs(fDenominator) > 1e-6) {
/* No, it's not parallel */
fDenominator *= fRayLen;
/* Bottom base */
fPotentialT[2] =
(m_cBasePos - c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;
/* Top base */
fPotentialT[3] =
(m_cBasePos + m_fHeight * m_cAxis - c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;
/* Make sure these t's are within the cylinder surface */
c_ray.GetPoint(cPoint, fPotentialT[2]);
CVector3 cDiff = cPoint - m_cBasePos;
if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))
fPotentialT[2] = -1;
c_ray.GetPoint(cPoint, fPotentialT[3]);
cDiff = cPoint - m_cBasePos;
if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))
fPotentialT[3] = -1;
}
else {
/* Yes, it's parallel - discard the intersections */
fPotentialT[2] = -1.0;
fPotentialT[3] = -1.0;
}
/* Go through all the potential t's and get the best */
f_t_on_ray = 2.0;
for(UInt32 i = 0; i < 4; ++i) {
if(fPotentialT[i] > 0.0f) {
f_t_on_ray = Min(f_t_on_ray, fPotentialT[i]);
}
}
/* Return true only if the intersection point is within the ray limits */
return (f_t_on_ray < 1.0f);
}
else {
/* Yes, ray and axis are parallel */
/* Projection of cCylBase2RayStart onto the axis */
Real fProj = cCylBase2RayStart.DotProduct(m_cAxis);
/* Radial vector */
CVector3 cRadial(cCylBase2RayStart);
cRadial -= fProj * m_cAxis;
Real fDist = cRadial.Length();
/* Is ray within the cylinder radius? */
if(fDist > m_fRadius) {
/* No, it's not */
return false;
}
/* If we get here, it's because the ray might intersect the cylinder bases */
Real fDenominator = cRayDir.DotProduct(m_cAxis) * fRayLen;
/* Create a buffer for the 2 potential intersection points */
Real fPotentialT[2];
/* Bottom base */
fPotentialT[0] =
(m_cBasePos-c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;
/* Top base */
fPotentialT[1] =
(m_cBasePos + m_fHeight * m_cAxis - c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;
/* Make sure these t's are within the cylinder surface */
CVector3 cPoint;
c_ray.GetPoint(cPoint, fPotentialT[0]);
CVector3 cDiff = cPoint - m_cBasePos;
if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))
fPotentialT[0] = -1;
c_ray.GetPoint(cPoint, fPotentialT[1]);
cDiff = cPoint - m_cBasePos;
if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))
fPotentialT[1] = -1;
/* Go through all the potential t's and get the best */
f_t_on_ray = 2.0;
for(UInt32 i = 0; i < 2; ++i) {
if(fPotentialT[i] > 0.0f) {
f_t_on_ray = Min(f_t_on_ray, fPotentialT[i]);
}
}
/* Return true only if the intersection point is within the ray limits */
return (f_t_on_ray < 1.0f);
}
}
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
}
