Panda3D
 All Classes Functions Variables Enumerations
obstacleAvoidance.cxx
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 }
 All Classes Functions Variables Enumerations