void CPFA_qt_user_functions::DrawPheromones() {
Real x, y, weight;
vector<CVector2> trail;
CColor trailColor = CColor::GREEN, pColor = CColor::GREEN;
for(size_t i = 0; i < loopFunctions.PheromoneList.size(); i++) {
x = loopFunctions.PheromoneList[i].GetLocation().GetX();
y = loopFunctions.PheromoneList[i].GetLocation().GetY();
if(loopFunctions.DrawTrails == 1) {
trail = loopFunctions.PheromoneList[i].GetTrail();
weight = loopFunctions.PheromoneList[i].GetWeight();
if(weight > 0.25 && weight <= 1.0) // [ 100.0% , 25.0% )
pColor = trailColor = CColor::GREEN;
else if(weight > 0.05 && weight <= 0.25) // [ 25.0% , 5.0% )
pColor = trailColor = CColor::YELLOW;
else // [ 5.0% , 0.0% ]
pColor = trailColor = CColor::RED;
CRay3 ray;
size_t j = 0;
for(j = 1; j < trail.size(); j++) {
ray = CRay3(CVector3(trail[j - 1].GetX(), trail[j - 1].GetY(), 0.01),
CVector3(trail[j].GetX(), trail[j].GetY(), 0.01));
DrawRay(ray, trailColor, 1.0);
}
DrawCylinder(CVector3(x, y, 0.0), CQuaternion(), loopFunctions.FoodRadius, 0.025, pColor);
} else {
weight = loopFunctions.PheromoneList[i].GetWeight();
if(weight > 0.25 && weight <= 1.0) // [ 100.0% , 25.0% )
pColor = CColor::GREEN;
else if(weight > 0.05 && weight <= 0.25) // [ 25.0% , 5.0% )
pColor = CColor::YELLOW;
else // [ 5.0% , 0.0% ]
pColor = CColor::RED;
DrawCylinder(CVector3(x, y, 0.0), CQuaternion(), loopFunctions.FoodRadius, 0.025, pColor);
}
}
}
void CCameraDefaultSensor::Update() {
/* vector of controller rays */
std::vector<std::pair<bool, CRay3> >& vecCheckedRays =
m_pcControllableEntity->GetCheckedRays();
/* sensor parameters */
CTransformationMatrix3 cWorldToAnchorTransform;
CTransformationMatrix3 cWorldToCameraTransform;
CTransformationMatrix3 cCameraToWorldTransform;
CVector3 cCameraLocation, cLookAt, cUp;
CVector3 cX, cY, cZ;
CVector3 cNearCenter, cNearTopLeft, cNearTopRight, cNearBottomLeft, cNearBottomRight;
CVector3 cFarCenter, cFarTopLeft, cFarTopRight, cFarBottomLeft, cFarBottomRight;
std::array<CPlane, 6> arrFrustumPlanes;
CVector3 cBoundingBoxMinCorner, cBoundingBoxMaxCorner;
CVector3 cBoundingBoxPosition, cBoundingBoxHalfExtents;
/* for each camera sensor */
for(SSensor& s_sensor : m_vecSensors) {
/* calculate transform matrices */
cWorldToAnchorTransform.SetFromComponents(s_sensor.Anchor.Orientation, s_sensor.Anchor.Position);
cWorldToCameraTransform = cWorldToAnchorTransform * s_sensor.Offset;
cCameraToWorldTransform = cWorldToCameraTransform.GetInverse();
/* calculate camera direction vectors */
cCameraLocation = cWorldToCameraTransform.GetTranslationVector();
cLookAt = cWorldToCameraTransform * CVector3::Z;
cUp = CVector3(0,-1,0); // -Y
cUp.Rotate(cWorldToCameraTransform.GetRotationMatrix());
/* calculate direction vectors */
cZ = cCameraLocation - cLookAt;
cZ.Normalize();
cX = cUp;
cX.CrossProduct(cZ);
cX.Normalize();
cY = cZ;
cY.CrossProduct(cX);
/* calculate frustum coordinates */
cNearCenter = cCameraLocation - cZ * s_sensor.Range.GetMin();
cFarCenter = cCameraLocation - cZ * s_sensor.Range.GetMax();
cNearTopLeft = cNearCenter + (cY * s_sensor.NearPlaneHeight) - (cX * s_sensor.NearPlaneWidth);
cNearTopRight = cNearCenter + (cY * s_sensor.NearPlaneHeight) + (cX * s_sensor.NearPlaneWidth);
cNearBottomLeft = cNearCenter - (cY * s_sensor.NearPlaneHeight) - (cX * s_sensor.NearPlaneWidth);
cNearBottomRight = cNearCenter - (cY * s_sensor.NearPlaneHeight) + (cX * s_sensor.NearPlaneWidth);
cFarTopLeft = cFarCenter + (cY * s_sensor.FarPlaneHeight) - (cX * s_sensor.FarPlaneWidth);
cFarTopRight = cFarCenter + (cY * s_sensor.FarPlaneHeight) + (cX * s_sensor.FarPlaneWidth);
cFarBottomLeft = cFarCenter - (cY * s_sensor.FarPlaneHeight) - (cX * s_sensor.FarPlaneWidth);
cFarBottomRight = cFarCenter - (cY * s_sensor.FarPlaneHeight) + (cX * s_sensor.FarPlaneWidth);
/* show frustum if enabled by adding outline to the checked rays vector */
if(m_bShowFrustum) {
vecCheckedRays.emplace_back(false, CRay3(cNearTopLeft, cNearTopRight));
vecCheckedRays.emplace_back(false, CRay3(cNearTopRight, cNearBottomRight));
vecCheckedRays.emplace_back(false, CRay3(cNearBottomRight, cNearBottomLeft));
vecCheckedRays.emplace_back(false, CRay3(cNearBottomLeft, cNearTopLeft));
vecCheckedRays.emplace_back(false, CRay3(cFarTopLeft, cFarTopRight));
vecCheckedRays.emplace_back(false, CRay3(cFarTopRight, cFarBottomRight));
vecCheckedRays.emplace_back(false, CRay3(cFarBottomRight, cFarBottomLeft));
vecCheckedRays.emplace_back(false, CRay3(cFarBottomLeft, cFarTopLeft));
vecCheckedRays.emplace_back(false, CRay3(cNearTopLeft, cFarTopLeft));
vecCheckedRays.emplace_back(false, CRay3(cNearTopRight, cFarTopRight));
vecCheckedRays.emplace_back(false, CRay3(cNearBottomRight, cFarBottomRight));
vecCheckedRays.emplace_back(false, CRay3(cNearBottomLeft, cFarBottomLeft));
}
//std::cerr << cFarBottomRight.GetZ() << "\t" << cFarBottomLeft.GetZ() << std::endl; TODO
/* generate a bounding box for the frustum */
cBoundingBoxMinCorner = cNearCenter;
cBoundingBoxMaxCorner = cNearCenter;
for(const CVector3& c_point : {
cNearTopLeft, cNearTopRight, cNearBottomLeft, cNearBottomRight,
cFarTopLeft, cFarTopRight, cFarBottomLeft, cFarBottomRight
}) {
if(c_point.GetX() > cBoundingBoxMaxCorner.GetX()) {
cBoundingBoxMaxCorner.SetX(c_point.GetX());
}
if(c_point.GetX() < cBoundingBoxMinCorner.GetX()) {
cBoundingBoxMinCorner.SetX(c_point.GetX());
}
if(c_point.GetY() > cBoundingBoxMaxCorner.GetY()) {
cBoundingBoxMaxCorner.SetY(c_point.GetY());
}
if(c_point.GetY() < cBoundingBoxMinCorner.GetY()) {
cBoundingBoxMinCorner.SetY(c_point.GetY());
}
if(c_point.GetZ() > cBoundingBoxMaxCorner.GetZ()) {
cBoundingBoxMaxCorner.SetZ(c_point.GetZ());
}
if(c_point.GetZ() < cBoundingBoxMinCorner.GetZ()) {
cBoundingBoxMinCorner.SetZ(c_point.GetZ());
}
}
cBoundingBoxMaxCorner *= 0.5;
cBoundingBoxMinCorner *= 0.5;
cBoundingBoxPosition = (cBoundingBoxMaxCorner + cBoundingBoxMinCorner);
cBoundingBoxHalfExtents = (cBoundingBoxMaxCorner - cBoundingBoxMinCorner);
/* generate frustum planes */
arrFrustumPlanes[0].SetFromThreePoints(cNearTopRight, cNearTopLeft, cFarTopLeft);
arrFrustumPlanes[1].SetFromThreePoints(cNearBottomLeft, cNearBottomRight, cFarBottomRight);
arrFrustumPlanes[2].SetFromThreePoints(cNearTopLeft, cNearBottomLeft, cFarBottomLeft);
arrFrustumPlanes[3].SetFromThreePoints(cNearBottomRight, cNearTopRight, cFarBottomRight);
arrFrustumPlanes[4].SetFromThreePoints(cNearTopLeft, cNearTopRight, cNearBottomRight);
arrFrustumPlanes[5].SetFromThreePoints(cFarTopRight, cFarTopLeft, cFarBottomLeft);
/* execute each algorithm */
for(CCameraSensorSimulatedAlgorithm* pc_algorithm : s_sensor.Algorithms) {
pc_algorithm->Update(s_sensor.ProjectionMatrix,
arrFrustumPlanes,
cCameraToWorldTransform,
cCameraLocation,
cBoundingBoxPosition,
cBoundingBoxHalfExtents);
/* transfer any rays to the controllable entity for rendering */
vecCheckedRays.insert(std::end(vecCheckedRays),
std::begin(pc_algorithm->GetCheckedRays()),
std::end(pc_algorithm->GetCheckedRays()));
}
}
}
