// Applies the three laws to the flock of boids void Boid::flock(vector<Boid> v) { Pvector sep = Separation(v); Pvector ali = Alignment(v); Pvector coh = Cohesion(v); // Arbitrarily weight these forces sep.mulScalar(SepW); ali.mulScalar(AliW); // Might need to alter weights for different characteristics coh.mulScalar(CohW); // Add the force vectors to acceleration applyForce(sep); applyForce(ali); applyForce(coh); }
void Small::Update(int mousex, int mousey) { //XMVECTOR playerlocation = {playerx, playery, 0, 0}; //float displacement = XMVectorGetX(XMVector2Length(playerlocation - Location)); //if (displacement <= 130.0f) //{ //Separation(0.000f); //Cohesion(0.000f); //Alignment(0.0f); //Chase(0.03f); //} //else //{ if (Flocking) { Separation(0.04f); Cohesion(0.01f); Alignment(0.01f); } else { //Wander } if (Chasing) { Chase(0.05f); } BorderRestrict(); if (Chasing) { Location += (Direction * 3.3f); } PointingTwoLocation = Location + (Direction * 10.0f); }
Vector2 TikiSteering::CalculatePrioritized() { Vector2 force = Vector2::Zero; if (On(separation)) { force = Separation() * weightSeparation; if (!AccumulateForce(steeringForce, force)) return steeringForce; } if(On(seek)) { force = Seek(target) * weightSeek; if (!AccumulateForce(steeringForce, force)) return steeringForce; } if (On(arrive)) { force = Arrive(target, deceleration) * weightArrive; if (!AccumulateForce(steeringForce, force)) return steeringForce; } if (On(wander)) { force = Wander() * weightWander; if (!AccumulateForce(steeringForce, force)) return steeringForce; } return steeringForce; }
/* * hex: 48 20 16 01 05 00 00 00 07 08 34 56 67 78 21 48 20 16 01 05 00 00 00 00 07 08 34 56 67 78 21 98 */ int deviceHeartPacket(char *packet, int length, char* info) { char u8DeviceId[LEN_DEVICE_ID*2+1] = {0}; char u8RelationShipDevice[LEN_DEVICE_ID*2+6] = {0}; char u8RedisUserValue[REDIS_COMMAND_LENGTH] = {0}; char u8RedisDeviceInfo[REDIS_COMMAND_LENGTH] = {0}; char u8DeviceInfo[REDIS_COMMAND_LENGTH] = {0}; if(length != LEN_HEART_PACKET_DEVICE) { return -3; } //device id hex to string hextostring(u8DeviceId ,packet+IDX_DEVICE_ID, LEN_DEVICE_ID); //get phoneid by device id sprintf(u8RelationShipDevice, "R_%s", u8DeviceId); apollo_printf(" u8RelationShipDevice:%s\n", u8RelationShipDevice); if (-1 == get_value_fromredis(u8RelationShipDevice, u8RedisUserValue)) { // device id don't bind return -1; } apollo_printf(" timeDiff: %d\n", *(int*)(packet+24)); #if ENABLE_FALLDOWN_HTTPPOST if((*(packet+10) & 0x0F)) { extern void *http_device_falldown(void *data); pthread_t tid = -1; int err = pthread_create(&tid, NULL, http_device_falldown, (void *)packet); if(0 != err) fprintf(stderr, "Couldn't run thread numbe, errno %d\n", err); else fprintf(stderr, "Thread\n"); } #endif //set device info {device->User: 1->1} hextostring(u8DeviceInfo, packet+IDX_DEVICE_POWER, LEN_DEVICE_INFO); sprintf(u8RedisDeviceInfo, "I_%s_%d", u8DeviceInfo, *((int*)(packet+IDX_DEVICE_HEARTPACKET_TIMESTAMP))); apollo_printf(" u8RedisDeviceInfo:%s, u8RedisUserValue:%s\n", u8RedisDeviceInfo, u8RedisUserValue); if (-1 == set_key_toredis(u8RedisUserValue, u8RedisDeviceInfo)) { return -2; } //get User Cmd info memset(u8RedisDeviceInfo, 0, REDIS_COMMAND_LENGTH); if (-1 == get_value_fromredis(u8DeviceId, u8RedisDeviceInfo)) { return 0; //No Cmd } else { int len = strlen(u8RedisDeviceInfo); if (strlen(u8RedisDeviceInfo) < 4) { int cmd = atoi(u8RedisDeviceInfo); apollo_printf("cmd:%s\n", u8RedisDeviceInfo); del_redis_key(u8DeviceId); return (cmd < 0 || cmd > 254) ? -4 : cmd; } else { int Count = 0, i; int cmd = 0; int index = 0; char u8PhoneNumber[LEN_PHONE_NUMBER+1]; char **Param = (char**)malloc(sizeof(char**)); Separation('_', u8RedisDeviceInfo, &Param, &Count); //printf(" Count:%d\n", Count); cmd = atoi(Param[0]); //cmd apollo_printf(" int cmd:%d", cmd); info[IDX_DEVICE_RETURN_COMMAND] = (char)cmd; if ((info[IDX_DEVICE_RETURN_COMMAND] & 0xF0) == 0x40) { index = atoi(Param[1]); info[IDX_DEVICE_RETURN_INDEX] = (char)index; if (Count == 3){ //phone number //int phoneLen = strlen(Param[2]); int i = 0; for (i = 0;i < LEN_PHONE_NUMBER;i ++) { u8PhoneNumber[i] = STRINGTOHEX(Param[2][2*i], Param[2][2*i+1]); //printf("%c%c %x \n",Param[2][2*i], Param[2][2*i+1], u8PhoneNumber[i]); } memcpy(info+IDX_DEVICE_RETURN_PHNUM, u8PhoneNumber, LEN_PHONE_NUMBER); } } del_redis_key(u8DeviceId); for (i = 0;i < Count;i ++) { free(Param[i]); } free(Param); apollo_printf(" aaaaa "); return cmd & 0xF0; } } return 0; }
inline Angle Separation( const Horizontal & hor0, const Horizontal & hor1 ) { return Separation( hor0.m_spherical, hor1.m_spherical ); }
//---------------------- CalculateWeightedSum ---------------------------- // // this simply sums up all the active behaviors X their weights and // truncates the result to the max available steering force before // returning //------------------------------------------------------------------------ ofVec3f SteeringBehaviors::CalculateWeightedSum() { if (On(wall_avoidance)) { m_SteeringForce += WallAvoidance(m_Vehicle->GameWorld()->getWalls()) * m_WeightObstacleAvoidance; } if (On(obstacle_avoidance)) { m_SteeringForce += ObstacleAvoidance( m_Vehicle->GameWorld()->getObstacles() ) * m_WeightObstacleAvoidance; } if (On(seek)) { m_SteeringForce += Seek(m_Vehicle->Target()->Pos()) * m_WeightSeek; } if (On(flee)) { m_SteeringForce += Flee(m_Vehicle->Target()->Pos()) * m_WeightFlee; } if (On(arrive)) { m_SteeringForce += Arrive(m_Vehicle->Target()->Pos(), normal) * m_WeightArrive; } if (!isSpacePartioningOn()) { if (On(separation)) { m_SteeringForce += Separation(m_Vehicle->rGroup()) * m_WeightSeparation; } if (On(alignment)) { m_SteeringForce += Alignment(m_Vehicle->rGroup()) * m_WeightAlignment; } if (On(cohesion)) { m_SteeringForce += Cohesion(m_Vehicle->rGroup()) * m_WeightCohesion; } } else { if (On(separation)) { m_SteeringForce += SeparationPlus(m_Vehicle->rGroup()) * m_WeightSeparation; } if (On(alignment)) { m_SteeringForce += AlignmentPlus(m_Vehicle->rGroup()) * m_WeightAlignment; } if (On(cohesion)) { m_SteeringForce += CohesionPlus(m_Vehicle->rGroup()) * m_WeightCohesion; } } if (On(repulsion)) { //need to retag the guys! m_SteeringForce += Repulsion(m_Vehicle->oGroup()) * m_WeightRepulsion; } if (On(wander)) { m_SteeringForce += Wander() * m_WeightWander; } if (On(follow_path)) { m_SteeringForce += FollowPath() * m_WeightFollowPath; } m_SteeringForce.limit(m_Vehicle->MaxForce()); return m_SteeringForce; }
//---------------------- CalculatePrioritized ---------------------------- // // this method calls each active steering behavior in order of priority // and acumulates their forces until the max steering force magnitude // is reached, at which time the function returns the steering force // accumulated to that point //------------------------------------------------------------------------ ofVec3f SteeringBehaviors::CalculatePrioritized() { ofVec3f force; if (On(wall_avoidance)) { force = WallAvoidance(m_Vehicle->GameWorld()->getWalls()) * m_WeightObstacleAvoidance; if (!AccumulateForce(m_SteeringForce, force)) return m_SteeringForce; } if (On(obstacle_avoidance)) { force = ObstacleAvoidance(m_Vehicle->GameWorld()->getObstacles()) * m_WeightObstacleAvoidance; if (!AccumulateForce(m_SteeringForce, force)) return m_SteeringForce; } if (On(flee)) { force = Flee(m_Vehicle->Target()->Pos()) * m_WeightFlee; if (!AccumulateForce(m_SteeringForce, force)) return m_SteeringForce; } if (On(repulsion)) { //need to retage the guys! force = Repulsion(m_Vehicle->oGroup()) * m_WeightRepulsion; if (!AccumulateForce(m_SteeringForce, force)) return m_SteeringForce; } if (!isSpacePartioningOn()) { if (On(separation)) { force = Separation(m_Vehicle->rGroup()) * m_WeightSeparation; if (!AccumulateForce(m_SteeringForce, force)) return m_SteeringForce; } if (On(alignment)) { force = Alignment(m_Vehicle->rGroup()) * m_WeightAlignment; if (!AccumulateForce(m_SteeringForce, force)) return m_SteeringForce; } if (On(cohesion)) { force = Cohesion(m_Vehicle->rGroup()) * m_WeightCohesion; if (!AccumulateForce(m_SteeringForce, force)) return m_SteeringForce; } } else { if (On(separation)) { force = SeparationPlus(m_Vehicle->rGroup()) * m_WeightSeparation; if (!AccumulateForce(m_SteeringForce, force)) return m_SteeringForce; } if (On(alignment)) { force = AlignmentPlus(m_Vehicle->rGroup()) * m_WeightAlignment; if (!AccumulateForce(m_SteeringForce, force)) return m_SteeringForce; } if (On(cohesion)) { force = CohesionPlus(m_Vehicle->rGroup()) * m_WeightCohesion; if (!AccumulateForce(m_SteeringForce, force)) return m_SteeringForce; } } if (On(seek)) { force = Seek(m_Vehicle->Target()->Pos()) * m_WeightSeek; if (!AccumulateForce(m_SteeringForce, force)) return m_SteeringForce; } if (On(arrive)) { force = Arrive(m_Vehicle->Target()->Pos(), normal) * m_WeightArrive; if (!AccumulateForce(m_SteeringForce, force)) return m_SteeringForce; } if (On(wander)) { force = Wander() * m_WeightWander; if (!AccumulateForce(m_SteeringForce, force)) return m_SteeringForce; } if (On(follow_path)) { force = FollowPath() * m_WeightFollowPath; if (!AccumulateForce(m_SteeringForce, force)) return m_SteeringForce; } return m_SteeringForce; }
Vector2D SteeringBehavior::CalculatePrioritized() { Vector2D force; //if (On(wall_avoidance)) //{ // force = WallAvoidance(m_pVehicle->World()->Walls()) * // m_dWeightWallAvoidance; // if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce; //} //if (On(obstacle_avoidance)) //{ // force = ObstacleAvoidance(m_pVehicle->World()->Obstacles()) * // m_dWeightObstacleAvoidance; // if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce; //} //if (On(evade)) //{ // assert(m_pTargetAgent1 && "Evade target not assigned"); // force = Evade(m_pTargetAgent1) * m_dWeightEvade; // if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce; //} if (On(flee)) { force = Flee(vehicle_->GetGameWorld()->Agents()) * m_dWeightFlee; if (!AccumulateForce(steering_force_, force)) return steering_force_; } // The next three can be combined for flocking behavior (wander is // Also a good behavior to add into this mix) if (On(separation)) { force = Separation(vehicle_->GetGameWorld()->Agents()) * m_dWeightSeparation; if (!AccumulateForce(steering_force_, force)) return steering_force_; } if (On(alignment)) { force = Alignment(vehicle_->GetGameWorld()->Agents()) * m_dWeightAlignment; if (!AccumulateForce(steering_force_, force)) return steering_force_; } if (On(cohesion)) { force = Cohesion(vehicle_->GetGameWorld()->Agents()) * m_dWeightCohesion; if (!AccumulateForce(steering_force_, force)) return steering_force_; } //if (On(seek)) //{ // force = Seek(m_pVehicle->World()->Crosshair()) * m_dWeightSeek; // if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce; //} //if (On(arrive)) //{ // force = Arrive(m_pVehicle->World()->Crosshair(), m_Deceleration) * m_dWeightArrive; // if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce; //} if (On(wander)) { force = Wander() * weight_wander_; if (!AccumulateForce(steering_force_, force)) return steering_force_; } if (On(pursuit)) { //assert(target_agent_ && "pursuit target not assigned"); force = Pursuit(target_agent_) * m_dWeightPursuit; if (!AccumulateForce(steering_force_, force)) return steering_force_; } /* if (On(offset_pursuit)) { assert(m_pTargetAgent1 && "pursuit target not assigned"); assert(!m_vOffset.isZero() && "No offset assigned"); force = OffsetPursuit(m_pTargetAgent1, m_vOffset); if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce; } if (On(interpose)) { assert(m_pTargetAgent1 && m_pTargetAgent2 && "Interpose agents not assigned"); force = Interpose(m_pTargetAgent1, m_pTargetAgent2) * m_dWeightInterpose; if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce; } if (On(hide)) { assert(m_pTargetAgent1 && "Hide target not assigned"); force = Hide(m_pTargetAgent1, m_pVehicle->World()->Obstacles()) * m_dWeightHide; if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce; } if (On(follow_path)) { force = FollowPath() * m_dWeightFollowPath; if (!AccumulateForce(m_vSteeringForce, force)) return m_vSteeringForce; }*/ return steering_force_; }
Vec2 SteeringForce::Calculate() { m_vSteeringForce = Vec2::ZERO; Vec2 force; std::vector<Vehicle*> neighbors; if (On(allignment)||On(separation)||On(cohesion)) { if (m_pVehicle->isCellSpaceOn()) { CellSpacePartition<Vehicle*>* cellSpace = GameData::Instance()->getCellSpace(); neighbors = cellSpace->getNeighbors(); }else{ auto data = GameData::Instance()->getEntityVector(); neighbors = tagNeighbors(m_pVehicle,data,SearchRad); } } if (On(wall_avoidance)) { auto data = GameData::Instance()->getWallls(); force = WallAvoidance(data); if (!AccumulateForce(m_vSteeringForce, force)) { return m_vSteeringForce; } } if (On(obstacle_avoidance)) { auto data = GameData::Instance()->getObstacle(); force = ObstacleAvoidance(data); if (!AccumulateForce(m_vSteeringForce, force)) { return m_vSteeringForce; } } if (On(separation)) { force = Separation(neighbors); if (!AccumulateForce(m_vSteeringForce, force)) { return m_vSteeringForce; } } if (On(cohesion)) { force = Cohesion(neighbors); if (!AccumulateForce(m_vSteeringForce, force)) { return m_vSteeringForce; } } if (On(allignment)) { force = Alignment(neighbors); if (!AccumulateForce(m_vSteeringForce, force)) { return m_vSteeringForce; } } if (On(hide)) { CCASSERT(m_pVehicle->getHideTarget()!=nullptr, "不存在躲避目标"); auto data = GameData::Instance()->getEntityVector(); force = Hide(m_pVehicle->getHideTarget(),data); if (!AccumulateForce(m_vSteeringForce, force)) { return m_vSteeringForce; } } if (On(follow_path)) { force = PathFollow(); if (!AccumulateForce(m_vSteeringForce, force)) { return m_vSteeringForce; } } if (On(seek)) { force = Seek(m_pVehicle->getTarget()); if (!AccumulateForce(m_vSteeringForce, force)) { return m_vSteeringForce; } } if (On(flee)) { force = Flee(m_pVehicle->getTarget()); if (!AccumulateForce(m_vSteeringForce, force)) { return m_vSteeringForce; } } if (On(arrive)) { force = Arrive(m_pVehicle->getTarget(), fast); if (!AccumulateForce(m_vSteeringForce, force)) { return m_vSteeringForce; } } if (On(offset_pursuit)) { force = OffsetPursuit(m_pVehicle->getLeader(), m_pVehicle->getOffsetToLeader()); if (!AccumulateForce(m_vSteeringForce, force)) { return m_vSteeringForce; } } if (On(pursuit)) { force = Pursuit(m_pVehicle->getEvaderv()); if (!AccumulateForce(m_vSteeringForce, force)) { return m_vSteeringForce; } } if (On(evade)) { force = Evade(m_pVehicle->getPursuer()); if (!AccumulateForce(m_vSteeringForce, force)) { return m_vSteeringForce; } } if (On(wander)) { force = Wander(); if (!AccumulateForce(m_vSteeringForce, force)) { return m_vSteeringForce; } } if (On(interpose)) { force = Interpose(m_pVehicle->getInterposeA(), m_pVehicle->getInterposeB()); if (!AccumulateForce(m_vSteeringForce, force)) { return m_vSteeringForce; } } return m_vSteeringForce; }
bool Spherical::Test( ) { bool ok = true; cout << "Testing Spherical" << endl; cout << "Spherical( ) [default constructor]" << endl; Spherical spherical0; cout << "Set( )" << endl; spherical0.Set( ); TESTCHECK( spherical0.Longitude().Radians(), 0., &ok ); TESTCHECK( spherical0.Latitude().Radians(), 0., &ok ); TESTCHECK( spherical0.Distance(), 0., &ok ); Point3D rect0 = spherical0.Rectangular(); TESTCHECK( rect0.X(), 0., &ok ); TESTCHECK( rect0.Y(), 0., &ok ); TESTCHECK( rect0.Z(), 0., &ok ); double lng = - M_PI / 2; Angle longitude( lng ); double lat = M_PI / 4; Angle latitude( lat ); double dist = sqrt( 2. ); cout << "Spherical( Angle(" << lng << "), Angle(" << lng << "), " << dist << " ) [longitude, latitude, distance constructor]" << endl; Spherical spherical1( longitude, latitude, dist ); TESTCHECK( spherical1.Longitude().Radians(), lng, &ok ); TESTCHECK( spherical1.Latitude().Radians(), lat, &ok ); TESTCHECK( spherical1.Distance(), dist, &ok ); Point3D rect = spherical1.Rectangular(); TESTCHECKF( rect.X(), 0., &ok ); TESTCHECKF( rect.Y(), -1., &ok ); TESTCHECKF( rect.Z(), 1., &ok ); lng = 3 * M_PI / 4; lat = 0.; dist = 4.; longitude.Set( lng ); latitude.Set( lat ); cout << "Set( Angle(" << lng << "), Angle(" << lng << "), " << dist << " )" << endl; spherical1.Set( longitude, latitude, dist ); TESTCHECK( spherical1.Longitude().Radians(), lng, &ok ); TESTCHECK( spherical1.Latitude().Radians(), lat, &ok ); TESTCHECK( spherical1.Distance(), dist, &ok ); rect = spherical1.Rectangular(); TESTCHECKF( rect.X(), - sqrt( 8. ), &ok ); TESTCHECKF( rect.Y(), sqrt( 8. ), &ok ); TESTCHECKF( rect.Z(), 0., &ok ); ostringstream ost; cout << "operator<<" << endl; ost << spherical1; TESTCHECK( ost.str(), string( "( 135°, 0°, 4 )" ), &ok ); double x = 3.; double y = 4.; double z = 0.; rect.Set( x, y, z ); cout << "Spherical( Point3D(" << x << ", " << y << ", " << z << ") ) [rectangular constructor]" << endl; Spherical spherical2( rect ); TESTCHECKF( spherical2.Longitude().Radians(), 0.927295218, &ok ); TESTCHECKF( spherical2.Latitude().Radians(), 0., &ok ); TESTCHECKF( spherical2.Distance(), 5., &ok ); rect = spherical2.Rectangular(); TESTCHECKF( rect.X(), x, &ok ); TESTCHECKF( rect.Y(), y, &ok ); TESTCHECKF( rect.Z(), z, &ok ); x = 12.; y = 0.; z = -5.; rect.Set( x, y, z ); cout << "Set( Point3D(" << x << ", " << y << ", " << z << ") )" << endl; spherical2.Set( rect ); TESTCHECKF( spherical2.Longitude().Radians(), 0., &ok ); TESTCHECKF( spherical2.Latitude().Radians(), -0.39479112, &ok ); TESTCHECKF( spherical2.Distance(), 13., &ok ); rect = spherical2.Rectangular(); TESTCHECKF( rect.X(), x, &ok ); TESTCHECKF( rect.Y(), y, &ok ); TESTCHECKF( rect.Z(), z, &ok ); int h = 14; int m = 15; double s = 39.7; cout << "ra = AngleHMS( " << h << ", " << m << ", " << s << " )" << endl; const Angle raArcturus( AngleHMS( h, m, s ) ); int d = 19; m = 10; s = 57.; cout << "dec = AngleDMS( " << d << ", " << m << ", " << s << " )" << endl; const Angle decArcturus( AngleDMS( d, m, s ) ); cout << "Arcturus: ( ra, dec, 1. )" << endl; const Spherical Arcturus( raArcturus, decArcturus ); h = 13; m = 25; s = 11.6; cout << "ra = AngleHMS( " << h << ", " << m << ", " << s << " )" << endl; const Angle raSpica( AngleHMS( h, m, s ) ); d = -11; m = 9; s = 41.; cout << "dec = AngleDMS( " << d << ", " << m << ", " << s << " )" << endl; const Angle decSpica( AngleDMS( d, m, s ) ); cout << "Spica: ( ra, dec, 1. )" << endl; const Spherical Spica( raSpica, decSpica ); cout << "Separation( Arcturus, Spica )" << endl; Angle sep = Separation( Arcturus, Spica ); TESTCHECKF( sep.Degrees(), 32.7930103, &ok ); lng = -1.5; lat = 0.75; dist = 40.; longitude.Set( lng ); latitude.Set( lat ); cout << "Set( Angle(" << lng << "), Angle(" << lat << "), " << dist << " )" << endl; spherical1.Set( longitude, latitude, dist ); TESTCHECK( ToJSON( spherical1 ), string( "{\n" "\"distance\": +4.00000000000000e+001,\n" "\"latitude\": +7.50000000000000e-001,\n" "\"longitude\": -1.50000000000000e+000\n" "}"), &ok ); cout << "FromJSON( ToJSON( spherical1 ), &spherical2 )" << endl; FromJSON( ToJSON( spherical1 ), &spherical2 ); TESTCHECK( spherical2.Longitude().Radians(), lng, &ok ); TESTCHECK( spherical2.Latitude().Radians(), lat, &ok ); TESTCHECK( spherical2.Distance(), dist, &ok ); if ( ok ) cout << "Spherical PASSED." << endl << endl; else cout << "Spherical FAILED." << endl << endl; return ok; }