00001 /* 00002 * flock.cpp 00003 * 00004 * Created on: Feb 4, 2012 00005 * Author: sushil 00006 */ 00007 00008 #include<climits> 00009 00010 #include <OgreVector3.h> 00011 00012 #include <engine.h> 00013 00014 #include <flock.h> 00015 #include <ent.h> 00016 #include <command.h> 00017 #include <unitAI.h> 00018 #include "DEBUG.h" 00019 00020 //#include <params.h> 00021 00022 FastEcslent::Flock::Flock(Group *grp, GroupTarget* trgt): Tactic(grp, FLOCK, trgt){ 00023 //group = grp; done in Tactic class 00024 //target = trgt; 00025 00026 } 00027 00028 inline bool FastEcslent::Flock::done() { 00029 return false; 00030 } 00031 00032 void FastEcslent::Flock::init(){ 00033 DEBUG(std::cout << "Initializing Flocking " << std::endl;) 00034 /* 00035 CohesionParameter = this->group->engine->flockParameters.cohesionParameter; 00036 AlignmentParameter = this->group->engine->flockParameters.alignmentParameter; 00037 SeparationParameter = this->group->engine->flockParameters.separationParameter; 00038 SeparationThreshold = this->group->engine->flockParameters.separationThreshold; 00039 00040 */ 00041 CohesionParameter = 1.0;//this->group->engine->flockParameters.cohesionParameter; 00042 AlignmentParameter = 1.0;//this->group->engine->flockParameters.alignmentParameter; 00043 SeparationParameter = 1.0;//this->group->engine->flockParameters.separationParameter; 00044 SeparationThreshold = 2000;//this->group->engine->flockParameters.separationThreshold; 00045 00046 00047 members = group->members; 00048 nMembers = group->nEntitiesInGroup; 00049 distanceMgr = this->group->engine->distanceMgr; 00050 entityMgr = this->group->engine->entityMgr; 00051 nEnts = entityMgr->nEnts; 00052 00053 for (int i = 0; i < nMembers; i++){ 00054 maxSpeedByAngle[i] = (members[i]->maxSpeed - members[i]->minSpeed)/1.57; //pi 00055 //members[i]->aspects.clear(); 00056 } 00057 00058 assert(group->leaderIndex >=0 && group->leaderIndex < group->nEntitiesInGroup); 00059 00060 DEBUG(std::cout << "Leader: " << group->members[group->leaderIndex]->uiname << " Number in group: " << group->nEntitiesInGroup << std::endl;) 00061 00062 } 00063 00064 00065 inline Ogre::Vector3 FastEcslent::Flock::cohesion(int i) { 00066 //steer towards avgPos 00067 Ogre::Vector3 force = avgPos - members[i]->pos; 00068 force.normalise(); 00069 00070 //steering force proportional to angleInRadians 00071 Ogre::Radian angleInRadians = avgPos.angleBetween(members[i]->pos); 00072 return force * angleInRadians.valueRadians() * CohesionParameter / Ogre::Math::PI; 00073 } 00074 00075 inline Ogre::Vector3 FastEcslent::Flock::alignment(int i) { 00076 //Steer in the average direction 00077 Ogre::Vector3 force = avgVel - members[i]->vel; 00078 force.normalise(); 00079 00080 //force proportional to difference in direction 00081 Ogre::Radian angleInRadians = avgVel.angleBetween(members[i]->vel); 00082 return force * angleInRadians.valueRadians() * AlignmentParameter / Ogre::Math::PI; 00083 } 00084 00085 inline void FastEcslent::Flock::computeSeparation(int leaderIndex) { 00086 float dist; 00087 int iId, jId; 00088 float param = 0; 00089 int largePrime = 499;//7411; 00090 float minMass = 500; 00091 //for(int i = 0; i < nMembers; i++){ 00092 for(int i = 0; i < nEnts; i++){//everyone is to be avoided 00093 //if(i == leaderIndex) continue; 00094 // if distance i, j < threshold push away 00095 iId = i;//members[i]->entityId.id; 00096 00097 for(int j = 0; j < nMembers; j++){ 00098 if (i == j) continue; 00099 jId = members[j]->entityId.id; 00100 if (distanceMgr->distance[iId][jId] < SeparationThreshold){ 00101 dist = distanceMgr->distance[iId][jId]; 00102 00103 param = minMass + (int) (entityMgr->ents[iId]->mass*entityMgr->ents[iId]->speed) % largePrime ; 00104 //forces[j] = forces[j] + (distanceMgr->normalizedDistanceVec[iId][jId] * ((((int) entityMgr->ents[i]->mass)%(int) SeparationParameter)/(0.0001f+(dist * dist)))) ; 00105 //forces[j] = forces[j] + (distanceMgr->normalizedDistanceVec[iId][jId] * ((((int) members[i]->mass)%(int) SeparationParameter)/(0.0001f+(dist * dist)))) ; 00106 //forces[j] = forces[j] + (distanceMgr->normalizedDistanceVec[iId][jId] * (members[i]->mass * SeparationParameter/(0.000001+(dist * dist)))) ; 00107 forces[j] = forces[j] + (distanceMgr->normalizedDistanceVec[iId][jId] * (param * SeparationParameter/(0.000001+ (dist*dist)))) ; 00108 //forces[j] = forces[j] + (distanceMgr->normalizedDistanceVec[iId][jId] * (SeparationParameter/(0.000001+(dist * dist)))) ; 00109 } 00110 } 00111 } 00112 return; 00113 } 00114 00115 inline void FastEcslent::Flock::applyForces(int leaderIndex) { 00116 00117 for (int i = 0; i < nMembers; i++){ 00118 if (i == leaderIndex) continue; 00119 members[i]->desiredHeading = -atan2(forces[i].z, forces[i].x); 00120 //members[i]->desiredSpeed = members[i]->minSpeed + ( maxSpeedByAngle[i]/ (0.0001f + fabs(members[i]->vel.angleBetween(forces[i]).valueRadians()))); 00121 members[i]->desiredSpeed = members[i]->minSpeed + ( (1.57 - (members[i]->vel.angleBetween(forces[i]).valueRadians())) * maxSpeedByAngle[i]); 00122 //members[i]->desiredSpeed = members[i]->maxSpeed; 00123 } 00124 00125 } 00126 00127 inline void FastEcslent::Flock::tick(double dt) { 00128 //update leader - later 00129 //update flock 00130 avgPos.x = 0.0; avgPos.y = 0.0; avgPos.z = 0.0; 00131 avgVel.x = 0.0; avgVel.y = 0.0; avgVel.z = 0.0; 00132 int leaderIndex = this->group->leaderIndex; 00133 float weight = 0.0f; 00134 float mass = 1.0f; 00135 //Average position and average velocity 00136 for (int i = 0; i < nMembers; i++) { 00137 //if (i == this->group->leaderIndex) continue; 00138 //mass = ((int) members[i]->mass); 00139 avgPos += (members[i]->pos * mass); 00140 avgVel += (members[i]->vel* mass); 00141 weight += mass; 00142 forces[i].x = 0.0; forces[i].y = 0.0; forces[i].z = 0.0; 00143 } 00144 avgPos = members[leaderIndex]->pos;// avgPos/weight; 00145 avgVel = members[leaderIndex]->vel;//avgVel/weight; 00146 //alignment and cohesion 00147 for (int i = 0; i < nMembers; i++) { 00148 if(i != leaderIndex){ 00149 forces[i] = forces[i] + alignment(i); 00150 forces[i] = forces[i] + cohesion(i); 00151 } 00152 } 00153 //spread 00154 computeSeparation(leaderIndex); 00155 //convert to dh, ds and apply 00156 applyForces(leaderIndex); 00157 return; 00158 }