Panda3D
|
00001 //////////////////////////////////////////////////////////////////////// 00002 // Filename : obstacleAvoidance.cxx 00003 // Created by : Deepak, John, Navin 00004 // Date : 10 Nov 09 00005 //////////////////////////////////////////////////////////////////// 00006 // 00007 // PANDA 3D SOFTWARE 00008 // Copyright (c) Carnegie Mellon University. All rights reserved. 00009 // 00010 // All use of this software is subject to the terms of the revised BSD 00011 // license. You should have received a copy of this license along 00012 // with this source code in a file named "LICENSE." 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 // Function : obstacle_detection 00029 // Description : This function checks if an obstacle is near to the AICharacter and 00030 // if an obstacle is detected returns true 00031 00032 ///////////////////////////////////////////////////////////////////////////////// 00033 00034 bool ObstacleAvoidance::obstacle_detection() { 00035 // Calculate the volume of the AICharacter with respect to render 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 // Check if it's the nearest obstacle, If so initialize as the nearest obstacle 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 // If the nearest obstacle will collide with our AICharacter then send obstacle detection as true 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 // Function : obstacle_avoidance_activate 00071 // Description : This function activates obstacle_avoidance if a obstacle 00072 // is detected 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 // Function : do_obstacle_avoidance 00086 // Description : This function returns the force necessary by the AICharacter to 00087 // avoid the nearest obstacle detected by obstacle_detection 00088 // function 00089 // NOTE : This assumes the obstacles are spherical 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 // The more closer the obstacle, the more force it generates 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 }