Panda3D
obstacleAvoidance.cxx
Go to the documentation of this file.
1 /**
2  * PANDA 3D SOFTWARE
3  * Copyright (c) Carnegie Mellon University. All rights reserved.
4  *
5  * All use of this software is subject to the terms of the revised BSD
6  * license. You should have received a copy of this license along
7  * with this source code in a file named "LICENSE."
8  *
9  * @file obstacleAvoidance.cxx
10  * @author Deepak, John, Navin
11  * @date 2009-11-10
12  */
13 
14 #include "obstacleAvoidance.h"
15 
16 #include "aiWorld.h"
17 
18 ObstacleAvoidance::
19 ObstacleAvoidance(AICharacter *ai_char, float feeler_length) {
20  _ai_char = ai_char;
21  _feeler = feeler_length;
22 }
23 
24 ObstacleAvoidance::
25 ~ObstacleAvoidance() {
26 }
27 
28 /**
29  * This function checks if an obstacle is near to the AICharacter and if an
30  * obstacle is detected returns true
31  */
34  // Calculate the volume of the AICharacter with respect to render
35  PT(BoundingVolume) np_bounds = _ai_char->get_node_path().get_bounds();
36  CPT(BoundingSphere) np_sphere = np_bounds->as_bounding_sphere();
37  LVecBase3 avoidance(0.0, 0.0, 0.0);
38  double distance = 0x7fff ;
39  double expanded_radius = 0;
40  LVecBase3 to_obstacle;
41  for(unsigned int i = 0; i < _ai_char->_world->_obstacles.size(); ++i) {
42  PT(BoundingVolume) bounds = _ai_char->_world->_obstacles[i].get_bounds();
43  CPT(BoundingSphere) bsphere = bounds->as_bounding_sphere();
44  LVecBase3 near_obstacle = _ai_char->_world->_obstacles[i].get_pos() - _ai_char->get_node_path().get_pos();
45  // Check if it's the nearest obstacle, If so initialize as the nearest
46  // obstacle
47  if((near_obstacle.length() < distance) && (_ai_char->_world->_obstacles[i].get_pos() != _ai_char->get_node_path().get_pos())) {
48  _nearest_obstacle = _ai_char->_world->_obstacles[i];
49  distance = near_obstacle.length();
50  expanded_radius = bsphere->get_radius() + np_sphere->get_radius();
51  }
52  }
53 
54  LVecBase3 feeler = _feeler * _ai_char->get_char_render().get_relative_vector(_ai_char->get_node_path(), LVector3::forward());
55  feeler.normalize();
56  feeler *= (expanded_radius + np_sphere->get_radius()) ;
57  to_obstacle = _nearest_obstacle.get_pos() - _ai_char->get_node_path().get_pos();
58  LVector3 line_vector = _ai_char->get_char_render().get_relative_vector(_ai_char->get_node_path(), LVector3::forward());
59  LVecBase3 project = (to_obstacle.dot(line_vector) * line_vector) / line_vector.length_squared();
60  LVecBase3 perp = project - to_obstacle;
61 
62  // If the nearest obstacle will collide with our AICharacter then send
63  // obstacle detection as true
64  if (_nearest_obstacle && (perp.length() < expanded_radius - np_sphere->get_radius()) && (project.length() < feeler.length())) {
65  return true;
66  }
67  return false;
68 }
69 
70 /**
71  * This function activates obstacle_avoidance if a obstacle is detected
72  */
75  if (obstacle_detection()) {
76  _ai_char->_steering->turn_off("obstacle_avoidance_activate");
77  _ai_char->_steering->turn_on("obstacle_avoidance");
78  }
79 }
80 
81 /**
82  * This function returns the force necessary by the AICharacter to avoid the
83  * nearest obstacle detected by obstacle_detection function NOTE : This
84  * assumes the obstacles are spherical
85  */
88  LVecBase3 offset = _ai_char->get_node_path().get_pos() - _nearest_obstacle.get_pos();
89  PT(BoundingVolume) bounds =_nearest_obstacle.get_bounds();
90  CPT(BoundingSphere) bsphere = bounds->as_bounding_sphere();
91  PT(BoundingVolume) np_bounds = _ai_char->get_node_path().get_bounds();
92  CPT(BoundingSphere) np_sphere = np_bounds->as_bounding_sphere();
93 
94  if (obstacle_detection()) {
95  LVecBase3 direction = _ai_char->get_char_render().get_relative_vector(_ai_char->get_node_path(), LVector3::forward());
96  direction.normalize();
97  float forward_component = offset.dot(direction);
98  LVecBase3 projection = forward_component * direction;
99  LVecBase3 perpendicular_component = offset - projection;
100  double p = perpendicular_component.length();
101  perpendicular_component.normalize();
102  LVecBase3 avoidance = perpendicular_component;
103  // The more closer the obstacle, the more force it generates
104  avoidance = (avoidance * _ai_char->get_max_force() * _ai_char->_movt_force) / (p + 0.01);
105  return avoidance;
106  }
107  _ai_char->_steering->turn_on("obstacle_avoidance_activate");
108  _ai_char->_steering->turn_off("obstacle_avoidance");
109  return LVecBase3(0, 0, 0);
110 }
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
void turn_on(std::string ai_type)
This function turns on any aiBehavior which is passed as a string.
void turn_off(std::string ai_type)
This function turns off any aiBehavior which is passed as a string.
This defines a bounding sphere, consisting of a center and a radius.
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
LPoint3 get_pos() const
Retrieves the translation component of the transform.
Definition: nodePath.cxx:1044
LVector3 get_relative_vector(const NodePath &other, const LVecBase3 &vec) const
Given that the indicated vector is in the coordinate system of the other node, returns the same vecto...
Definition: nodePath.cxx:1970
void obstacle_avoidance_activate()
This function activates obstacle_avoidance if a obstacle is detected.
bool obstacle_detection()
This function checks if an obstacle is near to the AICharacter and if an obstacle is detected returns...
LVecBase3 do_obstacle_avoidance()
This function returns the force necessary by the AICharacter to avoid the nearest obstacle detected b...
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.