Determine state based on whether or not the feared object has direct line of sight to us. / void determineByLineOfSight() { /* If neither flag set, seek self. */ if (!(attractorSet || detractorSet)) { state = SEEK_TARGET; myTargetX = x; myTargetY = y; return; } /* If ONLY one flag is set, do that one! */ if ((attractorSet ^ detractorSet) && !(attractorSet && detractorSet)) { if (attractorSet) { state = SEEK_TARGET; myTargetX = myAttractorX; myTargetY = myAttractorY; } else { state = FLEE_TARGET; myTargetX = myDetractorX; myTargetY = myDetractorY; } return; } /* Flee if detractor has line of sight to us. */ float p1x = this.x; float p1y = this.y; float p2x = myDetractorX; float p2y = myDetractorY; float dx = p2x - p1x; float dy = p2y - p1y; float distSq = dx*dx + dy*dy; for (Obstacle ob : obstacles) { float p3x = ob.x; float p3y = ob.y; float radius = ob.halfSize; //if (((p3x > p1x) && (p3x > p2x)) || // ((p3x < p1x) && (p3x < p2x)) || // ((p3y > p1y) && (p3y > p2y)) || // ((p3y < p1y) && (p3y < p2y)) //) { // continue; //} if ((p3x-radius > p1x && p3x-radius > p2x) || (p3x+radius < p1x && p3x+radius < p2x) || (p3y-radius > p1y && p3y-radius > p2y) || (p3y+radius < p1y && p3y+radius < p2y) ) { continue; } float a = distSq; float b = 2*( (p2x-p1x)*(p1x-p3x) + (p2y-p1y)*(p1y-p3y) ); //float c = p3x*p3x + p3y*p3y - 2*(p3x*p1x + p3y*p1y) - (radius*radius); float c = ((p1x-p3x)*(p1x-p3x)) + ((p1y-p3y)*(p1y-p3y)) - (radius*radius); float discriminant = (b*b) - (4*a*c); //if (id==0) println( // "I= " + (discriminant >= 0.0) + ", " + // "p1=(" + p1x + "," + p1y + "), " + // "p2=(" + p2x + "," + p2y + "), " + // "p3=(" + p3x + "," + p3y + "), " + // "disc=" + discriminant); /* If line of sight is blocked by any obstacle, seek target. */ if (discriminant >= 0.0) { if (DRAW_LINE_OF_SIGHT) { stroke(0.0, 1.0, 0.0); line(p1x, p1y, p2x, p2y); stroke(0.0); } state = SEEK_TARGET; myTargetX = myAttractorX; myTargetY = myAttractorY; return; } } /* Line of sight not blocked, hide. */ if (DRAW_LINE_OF_SIGHT) { stroke(1.0, 0.0, 0.0); line(p1x, p1y, p2x, p2y); stroke(0.0); } state = FLEE_TARGET; myTargetX = myDetractorX; myTargetY = myDetractorY; return; } /** Determine state based on whether or not the feared object is closer to us than the seek object. / void determineByClosest() { /* If neither flag set, seek self. */ if (!(attractorSet || detractorSet)) { state = SEEK_TARGET; myTargetX = x; myTargetY = y; return; } /* If ONLY one flag is set, do that one! */ if ((attractorSet ^ detractorSet) && !(attractorSet && detractorSet)) { if (attractorSet) { state = SEEK_TARGET; myTargetX = myAttractorX; myTargetY = myAttractorY; } else { state = FLEE_TARGET; myTargetX = myDetractorX; myTargetY = myDetractorY; } return; } /* Find the distance to the attractor and detractor, act on the closest. Detractor has a weight factor. */ float dxA = this.x - myAttractorX; float dyA = this.y - myAttractorY; float distSqA = (dxA*dxA + dyA*dyA); float dxD = this.x - myDetractorX; float dyD = this.y - myDetractorY; float distSqD = (dxD*dxD + dyD*dyD) * 0.5; if (distSqA < distSqD) { state = SEEK_TARGET; myTargetX = myAttractorX; myTargetY = myAttractorY; } else { state = FLEE_TARGET; myTargetX = myDetractorX; myTargetY = myDetractorY; } } void checkBounds() { if (x < WEST_WALL) { x = WEST_WALL; } else if (x > EAST_WALL) { x = EAST_WALL; } if (y < NORTH_WALL) { y = NORTH_WALL; } else if (y > SOUTH_WALL) { y = SOUTH_WALL; } } void behaviourSeekTargetAndAvoidObstacles(int time) { float[] seekVec = seek(time, myTargetX, myTargetY); float[] obstacleVec = obstacleAvoidance(time); float[] wanderVec = wander(time); float[] dir = new float[] { seekVec[0]*seekWeight + ((obstacleVec != null) ? obstacleVec[0]*obstacleWeight : 0) + ((wanderVec != null) ? wanderVec[0]*wanderWeight : 0), seekVec[1]*seekWeight + ((obstacleVec != null) ? obstacleVec[1]*obstacleWeight : 0) + ((wanderVec != null) ? wanderVec[1]*wanderWeight : 0) }; float denom = (float)Math.sqrt( seekVec[0]*seekVec[0] + seekVec[1]*seekVec[1]); if (denom == 0.0) { seekVec[0] = 0.0; seekVec[1] = 0.0; } else { seekVec[0] = seekVec[0]/denom; seekVec[1] = seekVec[1]/denom; } oldX = this.x; oldY = this.y; this.x += dir[0]*speed; this.y += dir[1]*speed; angle = atan2(y-oldY, x-oldX); } float[] seek(int time, float targetX, float targetY) { float[] dir = getDir(x, y, targetX, targetY); float dx = targetX-x; float dy = targetY-y; float distanceSquared = dx*dx + dy*dy; myDistanceToTarget = (float) Math.sqrt(distanceSquared); float mag = 0.0; if (myDistanceToTarget > 5.0) { float slowdownDistance = ((myDistanceToTarget-curiosityDistance)/100); mag = min(1.0, abs(slowdownDistance)); } dir[0] *= mag; dir[1] *= mag; return dir; }; float[] obstacleAvoidance(int time) { return avoidAll(time); } /** Scales and sums repulsive forces of all obstacles. / float[] avoidAll(int time) { if (obstacles.length == 0) { return null; } float[] sumVecs = new float[2]; sumVecs[0] = 0.0; sumVecs[1] = 0.0; for (Obstacle ob : obstacles) { float dx = ob.x - x; float dy = ob.y - y; float lengthSquared = dx*dx + dy*dy; float distance = (float) Math.sqrt(lengthSquared); if (distance != 0.0) { float mag = (ob.halfSize*ob.halfSize)/lengthSquared; sumVecs[0] -= min(1.0, (dx/distance) * mag); sumVecs[1] -= min(1.0, (dy/distance) * mag); } } for (Seeker s : seekers) { if (s == this) { continue; } float dx = s.x - x; float dy = s.y - y; float lengthSquared = dx*dx + dy*dy; float distance = (float) Math.sqrt(lengthSquared); if (distance != 0.0) { float mag = (s.size*s.size)/lengthSquared; sumVecs[0] -= min(1.0, (dx/distance) * mag); sumVecs[1] -= min(1.0, (dy/distance) * mag); } } return sumVecs; } /** Calculates the repulsive force against the closest obstacle. Avoidance force is strongest when radius of obstacle is equal to the distance between target and obstacle. / float[] avoidClosest(int time) { if (obstacles.length == 0) { return null; } float closestLengthSquared = Float.POSITIVE_INFINITY; Obstacle closestOb = getClosestObstacle(this.x, this.y); if (closestOb == null) { return null; } float dx = closestOb.x - x; float dy = closestOb.y - y; float lengthSquared = dx*dx + dy*dy; float lengthToTarget = (float) Math.sqrt(lengthSquared); float lengthOfReach = closestOb.halfSize; float mag = max(0, (lengthOfReach/lengthToTarget)*obstacleMag); float[] dir = getDir(x, y, closestOb.x, closestOb.y); dir[0] *= -mag; dir[1] *= -mag; return dir; } Obstacle getClosestObstacle(float x, float y) { float closestLengthSquared = Float.POSITIVE_INFINITY; Obstacle closestOb = null; for (Obstacle ob : obstacles) { float dx = ob.x - x; float dy = ob.y - y; float lengthSquared = dx*dx + dy*dy; if (lengthSquared < closestLengthSquared) { closestLengthSquared = lengthSquared; closestOb = ob; } } return closestOb; } float[] wander(int time) { float[] dir; if (myDistanceToTarget > 3*curiosityDistance) { dir = getDir(x, y, random(width), random(height)); } else { dir = null; } return dir; } /** Returns the unit length vector from (x0, y0) to (x1, y1). If the length of the vector is 0, returns (0.0, 0.0). / float[] getDir(float x0, float y0, float x1, float y1) { float dx = x1 - x0; float dy = y1 - y0; float denom = (float)Math.sqrt(dx*dx + dy*dy); if (denom == 0.0) { return new float[] {0.0, 0.0}; } else { return new float[] { min(1.0, dx/denom), min(1.0, dy/denom) }; } } float hideDistance = 100; /** 1. Locate all of the hiding places. 2. Find the closest one. 3. Seek that hiding place.

Source code: Flocking

Built with Processing