00001 00002 00003 00004 00005 00006 00007 00008 00009 00010 00011 00012 00014 00015 #include "obstacleAvoidance.h" 00016 00017 ObstacleAvoidance::ObstacleAvoidance(AICharacter *ai_char, float feeler_length) { 00018 _ai_char = ai_char; 00019 _feeler = feeler_length; 00020 } 00021 00022 ObstacleAvoidance::~ObstacleAvoidance() { 00023 } 00024 00030 00032 00033 bool ObstacleAvoidance::obstacle_detection() { 00034 // Calculate the volume of the AICharacter with respect to render 00035 PT(BoundingVolume) np_bounds = _ai_char->get_node_path().get_bounds(); 00036 CPT(BoundingSphere) np_sphere = np_bounds->as_bounding_sphere(); 00037 LVecBase3f avoidance(0.0, 0.0, 0.0); 00038 double distance = 0x7fff ; 00039 double expanded_radius; 00040 LVecBase3f to_obstacle; 00041 LVecBase3f prev_avoidance; 00042 for(unsigned int i = 0; i < _ai_char->_world->_obstacles.size(); ++i) { 00043 PT(BoundingVolume) bounds = _ai_char->_world->_obstacles[i].get_bounds(); 00044 CPT(BoundingSphere) bsphere = bounds->as_bounding_sphere(); 00045 LVecBase3f near_obstacle = _ai_char->_world->_obstacles[i].get_pos() - _ai_char->get_node_path().get_pos(); 00046 // Check if it's the nearest obstacle, If so initialize as the nearest obstacle 00047 if((near_obstacle.length() < distance) && (_ai_char->_world->_obstacles[i].get_pos() != _ai_char->get_node_path().get_pos())) { 00048 _nearest_obstacle = _ai_char->_world->_obstacles[i]; 00049 distance = near_obstacle.length(); 00050 expanded_radius = bsphere->get_radius() + np_sphere->get_radius(); 00051 } 00052 } 00053 LVecBase3f feeler = _feeler * _ai_char->get_char_render().get_relative_vector(_ai_char->get_node_path(), LVector3f::forward()); 00054 feeler.normalize(); 00055 feeler *= (expanded_radius + np_sphere->get_radius()) ; 00056 to_obstacle = _nearest_obstacle.get_pos() - _ai_char->get_node_path().get_pos(); 00057 LVector3f line_vector = _ai_char->get_char_render().get_relative_vector(_ai_char->get_node_path(), LVector3f::forward()); 00058 LVecBase3f project = (to_obstacle.dot(line_vector) * line_vector) / line_vector.length_squared(); 00059 LVecBase3f perp = project - to_obstacle; 00060 // If the nearest obstacle will collide with our AICharacter then send obstacle detection as true 00061 if((_nearest_obstacle) && (perp.length() < expanded_radius - np_sphere->get_radius()) && (project.length() < feeler.length())) { 00062 return true; 00063 } 00064 return false; 00065 } 00066 00072 00074 00075 void ObstacleAvoidance::obstacle_avoidance_activate() { 00076 if(obstacle_detection()) { 00077 _ai_char->_steering->turn_off("obstacle_avoidance_activate"); 00078 _ai_char->_steering->turn_on("obstacle_avoidance"); 00079 } 00080 } 00081 00089 00091 00092 LVecBase3f ObstacleAvoidance::do_obstacle_avoidance() { 00093 LVecBase3f offset = _ai_char->get_node_path().get_pos() - _nearest_obstacle.get_pos(); 00094 PT(BoundingVolume) bounds =_nearest_obstacle.get_bounds(); 00095 CPT(BoundingSphere) bsphere = bounds->as_bounding_sphere(); 00096 PT(BoundingVolume) np_bounds = _ai_char->get_node_path().get_bounds(); 00097 CPT(BoundingSphere) np_sphere = np_bounds->as_bounding_sphere(); 00098 double distance_needed = offset.length() - bsphere->get_radius() - np_sphere->get_radius(); 00099 if((obstacle_detection())) { 00100 LVecBase3f direction = _ai_char->get_char_render().get_relative_vector(_ai_char->get_node_path(), LVector3f::forward()); 00101 direction.normalize(); 00102 float forward_component = offset.dot(direction); 00103 LVecBase3f projection = forward_component * direction; 00104 LVecBase3f perpendicular_component = offset - projection; 00105 double p = perpendicular_component.length(); 00106 perpendicular_component.normalize(); 00107 LVecBase3f avoidance = perpendicular_component; 00108 // The more closer the obstacle, the more force it generates 00109 avoidance = (avoidance * _ai_char->get_max_force() * _ai_char->_movt_force) / (p + 0.01); 00110 return avoidance; 00111 } 00112 _ai_char->_steering->turn_on("obstacle_avoidance_activate"); 00113 _ai_char->_steering->turn_off("obstacle_avoidance"); 00114 return LVecBase3f(0, 0, 0); 00115 }