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