// Do line segments (r1p1.x, r1p1.y)--(r1p2.x, r1p2.y) and (r2p1.x, r2p1.y)--(r2p2.x, r2p2.y) intersect?
// from: http://ptspts.blogspot.com/2010/06/how-to-determine-if-two-line-segments.html
bool doLineSegmentsIntersect(glm::vec2 r1p1, glm::vec2 r1p2, glm::vec2 r2p1, glm::vec2 r2p2) {
int d1 = computeDirection(r2p1.x, r2p1.y, r2p2.x, r2p2.y, r1p1.x, r1p1.y);
int d2 = computeDirection(r2p1.x, r2p1.y, r2p2.x, r2p2.y, r1p2.x, r1p2.y);
int d3 = computeDirection(r1p1.x, r1p1.y, r1p2.x, r1p2.y, r2p1.x, r2p1.y);
int d4 = computeDirection(r1p1.x, r1p1.y, r1p2.x, r1p2.y, r2p2.x, r2p2.y);
return (((d1 > 0 && d2 < 0) || (d1 < 0 && d2 > 0)) &&
((d3 > 0 && d4 < 0) || (d3 < 0 && d4 > 0))) ||
(d1 == 0 && isOnSegment(r2p1.x, r2p1.y, r2p2.x, r2p2.y, r1p1.x, r1p1.y)) ||
(d2 == 0 && isOnSegment(r2p1.x, r2p1.y, r2p2.x, r2p2.y, r1p2.x, r1p2.y)) ||
(d3 == 0 && isOnSegment(r1p1.x, r1p1.y, r1p2.x, r1p2.y, r2p1.x, r2p1.y)) ||
(d4 == 0 && isOnSegment(r1p1.x, r1p1.y, r1p2.x, r1p2.y, r2p2.x, r2p2.y));
}
bool Branch::doLineSegmentsIntersect(QLineF line1, QLineF line2)
{
char d1 = computeDirection(line2.p1().x(), line2.p1().y(), line2.p2().x(), line2.p2().y(), line1.p1().x(), line1.p1().y());
char d2 = computeDirection(line2.p1().x(), line2.p1().y(), line2.p2().x(), line2.p2().y(), line1.p2().x(), line1.p2().y());
char d3 = computeDirection(line1.p1().x(), line1.p1().y(), line1.p2().x(), line1.p2().y(), line2.p1().x(), line2.p1().y());
char d4 = computeDirection(line1.p1().x(), line1.p1().y(), line1.p2().x(), line1.p2().y(), line2.p2().x(), line2.p2().y());
return (((d1 > 0 && d2 < 0) || (d1 < 0 && d2 > 0)) &&
((d3 > 0 && d4 < 0) || (d3 < 0 && d4 > 0))) ||
(d1 == 0 && isOnSegment(line2.p1().x(), line2.p1().y(), line2.p2().x(), line2.p2().y(), line1.p1().x(), line1.p1().y())) ||
(d2 == 0 && isOnSegment(line2.p1().x(), line2.p1().y(), line2.p2().x(), line2.p2().y(), line1.p2().x(), line1.p2().y())) ||
(d3 == 0 && isOnSegment(line1.p1().x(), line1.p1().y(), line1.p2().x(), line1.p2().y(), line2.p1().x(), line2.p1().y())) ||
(d4 == 0 && isOnSegment(line1.p1().x(), line1.p1().y(), line1.p2().x(), line1.p2().y(), line2.p2().x(), line2.p2().y()));
}
void CartSupplier::onDestination()
{
Walker::onDestination();
CityHelper helper( _d->city );
if( _getPathway().isReverse() )
{
// walker is back in the market
deleteLater();
// put the content of the stock to receiver
BuildingPtr building = helper.find<Building>( _d->baseBuildingPos );
GoodStore* storage = 0;
if( building.is<Factory>() )
{
storage = &building.as<Factory>()->getGoodStore();
}
else if( building.is<Granary>() )
{
storage = &building.as<Granary>()->getGoodStore();
}
else if( building.is<Warehouse>() )
{
storage = &building.as<Warehouse>()->getGoodStore();
}
if( storage )
{
storage->applyStorageReservation( _d->stock, _d->rcvReservationID );
storage->store( _d->stock, _d->stock._currentQty );
}
}
else
{
// walker is near the granary/warehouse
_getPathway().rbegin();
computeDirection();
go();
// get goods from destination building
BuildingPtr building = helper.find<Building>( _d->storageBuildingPos );
if( building.is<Granary>() )
{
GranaryPtr granary = building.as<Granary>();
// this is a granary!
// std::cout << "MarketBuyer arrives at granary, res=" << _reservationID << std::endl;
granary->getGoodStore().applyRetrieveReservation( _d->stock, _d->reservationID );
}
else if( building.is<Warehouse>() )
{
WarehousePtr warehouse = building.as<Warehouse>();
// this is a warehouse!
// std::cout << "Market buyer takes IRON from warehouse" << std::endl;
// warehouse->retrieveGoods(_basket.getStock(G_IRON));
warehouse->getGoodStore().applyRetrieveReservation(_d->stock, _d->reservationID);
}
}
}
void Walker::onMidTile()
{
// std::cout << "Walker is on mid tile! coord=" << _i << "," << _j << std::endl;
if (_pathWay.isDestination())
{
onDestination();
}
else
{
// compute the direction to reach the destination
computeDirection();
}
}
void PxVehicle4WEnable3WDeltaMode(PxVehicleWheelsSimData& wheelsSimData, PxVehicleWheelsDynData& wheelsDynData, PxVehicleDriveSimData4W& driveSimData)
{
PX_CHECK_AND_RETURN
(!wheelsSimData.getIsWheelDisabled(PxVehicleDrive4WWheelOrder::eFRONT_LEFT) &&
!wheelsSimData.getIsWheelDisabled(PxVehicleDrive4WWheelOrder::eFRONT_RIGHT) &&
!wheelsSimData.getIsWheelDisabled(PxVehicleDrive4WWheelOrder::eREAR_LEFT) &&
!wheelsSimData.getIsWheelDisabled(PxVehicleDrive4WWheelOrder::eREAR_RIGHT), "PxVehicle4WEnable3WDeltaMode requires no wheels to be disabled");
PxU32 rightDirection=0xffffffff;
PxU32 upDirection=0xffffffff;
computeDirection(rightDirection, upDirection);
PX_CHECK_AND_RETURN(rightDirection<3 && upDirection<3, "PxVehicle4WEnable3WTadpoleMode requires the vectors set in PxVehicleSetBasisVectors to be axis-aligned");
enable3WMode(rightDirection, upDirection, true, wheelsSimData, wheelsDynData, driveSimData);
}
void ServiceWalker::onDestination()
{
Walker::onDestination();
if (_pathWay.isReverse())
{
// walker is back in the market
_isDeleted= true;
}
else
{
// walker finished service => get back to service building
_pathWay.rbegin();
_action._action=WA_MOVE;
computeDirection();
}
}
void CartPusher::onDestination()
{
Walker::onDestination();
_d->cartPicture = NULL;
if( _d->consumerBuilding != NULL )
{
GranaryPtr granary = _d->consumerBuilding.as<Granary>();
WarehousePtr warehouse = _d->consumerBuilding.as<Warehouse>();
FactoryPtr factory = _d->consumerBuilding.as<Factory>();
if( granary != NULL )
{
granary->getGoodStore().applyStorageReservation(_d->stock, _d->reservationID);
granary->computePictures();
_d->reservationID = 0;
}
else if ( warehouse != NULL )
{
warehouse->getGoodStore().applyStorageReservation(_d->stock, _d->reservationID);
warehouse->computePictures();
_d->reservationID = 0;
}
else if( factory != NULL )
{
factory->getGoodStore().applyStorageReservation(_d->stock, _d->reservationID);
// factory->computePictures();
_d->reservationID = 0;
}
}
//
if( !_pathWay.isReverse() )
{
_pathWay.toggleDirection();
_action._action=WA_MOVE;
computeDirection();
_d->consumerBuilding = 0;
}
else
{
deleteLater();
}
}
Ray RayTracer::computeRay(uint x, uint y) {
Vect dir = computeDirection(x, y);
Ray r(getCameraPos(), dir);
return r;
}
void calcul_mission()
{
struct coordinates_ C_blue; //coordinates of drone
struct coordinates_ nextPoint;
struct coordinates_ startPoint;
mission_state_t state;
int indice = 0;
int index = 0;
int bad_move = 0;
float angle_actuel, calcul_x, calcul_y, angle_desire;
node_t **path = NULL;
C_blue.x = 0.0;
C_blue.y = 0.0;
// First of all, check if the given destination is valid
if(find_point(graph, destination.x, destination.y) == -1)
{
printf("Invalid destination: (%.2f, %.2f)\n", destination.x, destination.y);
state = FINISHED;
}
else
state = INIT;
while(state != FINISHED)
{
if(inC.flag_control_s == STATE_MANUAL)
{
state = FINISHED;
}
switch(state)
{
case INIT:
printf("[Mission state: INIT]\n");
// Wait for the data to stabilize
sleep(5);
read_data_bluetooth(&C_blue.x,&C_blue.y);
printf("BT location: X = %f, Y = %f\n", C_blue.x, C_blue.y);
printf("Destination point: (%f, %f)\n", destination.x, destination.y);
path = dijkstra(C_blue.x, C_blue.y, destination.x, destination.y, graph);
if (path == NULL)
state = LOST;
else
{
printf("Path to follow\n");
int i;
for(i = 0 ; path[i] != NULL ; i++)
{
indice = i;
printf("%s (%f,%f)\n", path[i]->name, path[i]->x, path[i]->y);
}
state = RUNNING;
}
break;
case LOST:
printf("[Mission state: LOST]\n");
sleep(3);
read_data_bluetooth(&C_blue.x,&C_blue.y);
index = find_closest_node(graph, C_blue.x, C_blue.y);
startPoint.x = graph->nodes[index].x;
startPoint.y = graph->nodes[index].y;
Main_Nav = return_navdata();
angle_actuel = Main_Nav.magneto.heading_fusion_unwrapped;
angle_desire = computeDesiredAngle(C_blue, startPoint);
computeOffsetMag(&angle_desire, nav_prec, nav_suiv);
yaw_power = computeDirection(angle_actuel, angle_desire, 0.3, &yaw_move);
if(rotate_to_desired_angle(angle_desire) == -1)
{
fprintf(stderr, "[%s:%d] Error: Rotation to desired angle failed\n", __FILE__, __LINE__);
break_drone();
stop_mission();
free(graph);
return;
}
// Let the drone stabilize itself
sleep(1);
bad_move = move_to(startPoint, &C_blue);
break_drone();
// If we find a start point, we can switch to state RUNNING
if(!bad_move)
{
printf("BT location: X = %f, Y = %f\n", C_blue.x, C_blue.y);
printf("Destination point: (%f, %f)\n", destination.x, destination.y);
path = dijkstra(C_blue.x, C_blue.y, destination.x, destination.y, graph);
if(path == NULL)
{
fprintf(stderr, "[%s:%d] Error: Bluetooth Data too unstable, aborting\n", __FILE__, __LINE__);
break_drone();
stop_mission();
free(graph);
return;
}
printf("Path to follow\n");
int i;
for(i = 0 ; path[i] != NULL ; i++)
{
indice = i;
printf("%s (%f,%f)\n", path[i]->name, path[i]->x, path[i]->y);
}
state = RUNNING;
}
break;
case DRIFTED:
printf("[Mission state: DRIFTED]\n");
// Pretty much the same that LOST, at the difference that we think that we know where we are
sleep(3);
read_data_bluetooth(&C_blue.x,&C_blue.y);
printf("BT location: X = %f, Y = %f\n", C_blue.x, C_blue.y);
printf("Destination point: (%f, %f)\n", destination.x, destination.y);
path = dijkstra(C_blue.x, C_blue.y, destination.x, destination.y, graph);
if (path == NULL)
{
// Mmh, Bluetooth data unstable, going to LOST
state = LOST;
}
else
{
printf("New path to follow\n");
int i;
for(i = 0 ; path[i] != NULL ; i++)
{
indice = i;
printf("%s (%f,%f)\n", path[i]->name, path[i]->x, path[i]->y);
}
state = RUNNING;
}
break;
case RUNNING:
printf("[Mission state: RUNNING]\n");
sleep(3);
read_data_bluetooth(&C_blue.x,&C_blue.y);
printf("BT location: X = %f, Y = %f\n", C_blue.x, C_blue.y);
printf("Destination point: (%f, %f)\n", destination.x, destination.y);
// Let's start
indice--;
if (indice <= 0)
{
state = ARRIVED;
}
else
{
// FIXME: Cast path[indice] from node_t to coordinates_ since we don't use the samee structure...
nextPoint.x = path[indice]->x;
nextPoint.y = path[indice]->y;
Main_Nav = return_navdata();
angle_actuel = Main_Nav.magneto.heading_fusion_unwrapped;
angle_desire = computeDesiredAngle(C_blue, nextPoint);
/*
* angle désiré
*/
//calcul the new angle_desire
computeOffsetMag(&angle_desire, nav_prec, nav_suiv);
//printf("Calcul angle desire final v1 : %2.f\n", angle_desire);
//calculating the direction of rotation of the Z-axis for optimum positioning.
yaw_power = computeDirection(angle_actuel, angle_desire, 0.3, &yaw_move);
//printf("Valeur de la puissance mise : %1.f, Valeur de l'angle souhaité = %2.f, valeur de l'angle actuel = %2.f sens de rotation = %d\n", yaw_power, angle_desire, angle_actuel, yaw_move);
if (rotate_to_desired_angle(angle_desire) == -1)
{
break_drone();
stop_mission();
free(graph);
return;
}
// Let the drone stabilize itself
sleep(1);
// Let's move to next point
bad_move = move_to_next_point(nextPoint, (const node_t **) path, &C_blue);
// Stop the drone movement
break_drone();
if(bad_move == 1)
state = LOST;
else if(bad_move == 2)
state = DRIFTED;
else
printf("One step made\n");
}
break;
case ARRIVED:
printf("[Mission state: ARRIVED]\n");
printf("You're arrived to destination\n");
state = FINISHED;
break;
case FINISHED:
// if stop mission called
break;
default:
fprintf(stderr, "[%s:%d] Error: Unknown mission state\n", __FILE__, __LINE__);
break_drone();
stop_mission();
free(graph);
return;
break;
}
}
printf("[Mission state: FINISHED]\n");
stop_mission();
printf("fin mission\n");
free(path);
}
void AvatarLogicCalculator::computeAndSetDirection(float destinationX, float destinationY)
{
std::pair<float,float> directions = computeDirection(destinationX, destinationY);
m_avatarModel->setStep(directions.first,directions.second);
}
void MarketLady::onDestination()
{
Walker::onDestination();
if( _getPathway().isReverse() )
{
// walker is back in the market
deleteLater();
// put the content of the basket in the market
_d->market->getGoodStore().storeAll( _d->basket );
}
else
{
// walker is near the granary/warehouse
_getPathway().rbegin();
_setAction( WA_MOVE );
computeDirection();
// get goods from destination building
LandOverlayPtr building = _d->city->getTilemap().at( _d->destBuildingPos ).getTerrain().getOverlay();
if( building.is<Granary>() )
{
GranaryPtr granary = building.as<Granary>();
// this is a granary!
// std::cout << "MarketLady arrives at granary, res=" << _reservationID << std::endl;
granary->getGoodStore().applyRetrieveReservation(_d->basket, _d->reservationID);
// take other goods if possible
for (int n = 1; n<G_MAX; ++n)
{
// for all types of good (except G_NONE)
GoodType goodType = (GoodType) n;
int qty = _d->market->getGoodDemand(goodType) - _d->basket.getCurrentQty(goodType);
if (qty != 0)
{
qty = std::min(qty, granary->getGoodStore().getMaxRetrieve(goodType));
qty = std::min(qty, _d->basket.getMaxQty(_d->priorityGood) - _d->basket.getCurrentQty(_d->priorityGood));
if (qty != 0)
{
// std::cout << "extra retrieve qty=" << qty << " basket=" << _basket.getStock(goodType)._currentQty << std::endl;
GoodStock& stock = _d->basket.getStock(goodType);
granary->getGoodStore().retrieve(stock, qty);
}
}
}
}
else if( building.is<Warehouse>() )
{
WarehousePtr warehouse = building.as<Warehouse>();
// this is a warehouse!
// std::cout << "Market buyer takes IRON from warehouse" << std::endl;
// warehouse->retrieveGoods(_basket.getStock(G_IRON));
warehouse->getGoodStore().applyRetrieveReservation(_d->basket, _d->reservationID);
// take other goods if possible
for (int n = 1; n<G_MAX; ++n)
{
// for all types of good (except G_NONE)
GoodType goodType = (GoodType) n;
int qty = _d->market->getGoodDemand(goodType) - _d->basket.getCurrentQty(goodType);
if (qty != 0)
{
qty = std::min(qty, warehouse->getGoodStore().getMaxRetrieve(goodType));
qty = std::min(qty, _d->basket.getMaxQty(_d->priorityGood) - _d->basket.getCurrentQty(_d->priorityGood));
if (qty != 0)
{
// std::cout << "extra retrieve qty=" << qty << " basket=" << _basket.getStock(goodType)._currentQty << std::endl;
GoodStock& stock = _d->basket.getStock(goodType);
warehouse->getGoodStore().retrieve(stock, qty);
}
}
}
}
unsigned long delay = 20;
while( _d->basket.getCurrentQty() > 100 )
{
for( int gtype=G_WHEAT; gtype <= G_WINE; gtype++ )
{
GoodStock& currentStock = _d->basket.getStock( (GoodType)gtype );
if( currentStock._currentQty > 0 )
{
MarketLadyHelperPtr boy = MarketLadyHelper::create( this );
GoodStock& boyBasket = boy->getBasket();
boyBasket._goodType = (GoodType)gtype;
boyBasket._maxQty = 100;
_d->basket.retrieve( boyBasket, math::clamp( currentStock._currentQty, 0, 100 ) );
boy->setDelay( delay );
delay += 20;
boy->send2City( _d->city, _d->market );
}
}
}
}
}
