00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "obstacleAvoidance.h"
00017
00018 ObstacleAvoidance::ObstacleAvoidance(AICharacter *ai_char, float feeler_length) {
00019 _ai_char = ai_char;
00020 _feeler = feeler_length;
00021 }
00022
00023 ObstacleAvoidance::~ObstacleAvoidance() {
00024 }
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 bool ObstacleAvoidance::obstacle_detection() {
00035
00036 PT(BoundingVolume) np_bounds = _ai_char->get_node_path().get_bounds();
00037 CPT(BoundingSphere) np_sphere = np_bounds->as_bounding_sphere();
00038 LVecBase3f avoidance(0.0, 0.0, 0.0);
00039 double distance = 0x7fff ;
00040 double expanded_radius;
00041 LVecBase3f to_obstacle;
00042 LVecBase3f prev_avoidance;
00043 for(unsigned int i = 0; i < _ai_char->_world->_obstacles.size(); ++i) {
00044 PT(BoundingVolume) bounds = _ai_char->_world->_obstacles[i].get_bounds();
00045 CPT(BoundingSphere) bsphere = bounds->as_bounding_sphere();
00046 LVecBase3f near_obstacle = _ai_char->_world->_obstacles[i].get_pos() - _ai_char->get_node_path().get_pos();
00047
00048 if((near_obstacle.length() < distance) && (_ai_char->_world->_obstacles[i].get_pos() != _ai_char->get_node_path().get_pos())) {
00049 _nearest_obstacle = _ai_char->_world->_obstacles[i];
00050 distance = near_obstacle.length();
00051 expanded_radius = bsphere->get_radius() + np_sphere->get_radius();
00052 }
00053 }
00054 LVecBase3f feeler = _feeler * _ai_char->get_char_render().get_relative_vector(_ai_char->get_node_path(), LVector3f::forward());
00055 feeler.normalize();
00056 feeler *= (expanded_radius + np_sphere->get_radius()) ;
00057 to_obstacle = _nearest_obstacle.get_pos() - _ai_char->get_node_path().get_pos();
00058 LVector3f line_vector = _ai_char->get_char_render().get_relative_vector(_ai_char->get_node_path(), LVector3f::forward());
00059 LVecBase3f project = (to_obstacle.dot(line_vector) * line_vector) / line_vector.length_squared();
00060 LVecBase3f perp = project - to_obstacle;
00061
00062 if((_nearest_obstacle) && (perp.length() < expanded_radius - np_sphere->get_radius()) && (project.length() < feeler.length())) {
00063 return true;
00064 }
00065 return false;
00066 }
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076 void ObstacleAvoidance::obstacle_avoidance_activate() {
00077 if(obstacle_detection()) {
00078 _ai_char->_steering->turn_off("obstacle_avoidance_activate");
00079 _ai_char->_steering->turn_on("obstacle_avoidance");
00080 }
00081 }
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093 LVecBase3f ObstacleAvoidance::do_obstacle_avoidance() {
00094 LVecBase3f offset = _ai_char->get_node_path().get_pos() - _nearest_obstacle.get_pos();
00095 PT(BoundingVolume) bounds =_nearest_obstacle.get_bounds();
00096 CPT(BoundingSphere) bsphere = bounds->as_bounding_sphere();
00097 PT(BoundingVolume) np_bounds = _ai_char->get_node_path().get_bounds();
00098 CPT(BoundingSphere) np_sphere = np_bounds->as_bounding_sphere();
00099 double distance_needed = offset.length() - bsphere->get_radius() - np_sphere->get_radius();
00100 if((obstacle_detection())) {
00101 LVecBase3f direction = _ai_char->get_char_render().get_relative_vector(_ai_char->get_node_path(), LVector3f::forward());
00102 direction.normalize();
00103 float forward_component = offset.dot(direction);
00104 LVecBase3f projection = forward_component * direction;
00105 LVecBase3f perpendicular_component = offset - projection;
00106 double p = perpendicular_component.length();
00107 perpendicular_component.normalize();
00108 LVecBase3f avoidance = perpendicular_component;
00109
00110 avoidance = (avoidance * _ai_char->get_max_force() * _ai_char->_movt_force) / (p + 0.01);
00111 return avoidance;
00112 }
00113 _ai_char->_steering->turn_on("obstacle_avoidance_activate");
00114 _ai_char->_steering->turn_off("obstacle_avoidance");
00115 return LVecBase3f(0, 0, 0);
00116 }