Panda3D
 All Classes Functions Variables Enumerations
obstacleAvoidance.cxx
1 ////////////////////////////////////////////////////////////////////////
2 // Filename : obstacleAvoidance.cxx
3 // Created by : Deepak, John, Navin
4 // Date : 10 Nov 09
5 ////////////////////////////////////////////////////////////////////
6 //
7 // PANDA 3D SOFTWARE
8 // Copyright (c) Carnegie Mellon University. All rights reserved.
9 //
10 // All use of this software is subject to the terms of the revised BSD
11 // license. You should have received a copy of this license along
12 // with this source code in a file named "LICENSE."
13 //
14 ////////////////////////////////////////////////////////////////////
15 
16 #include "obstacleAvoidance.h"
17 
18 ObstacleAvoidance::ObstacleAvoidance(AICharacter *ai_char, float feeler_length) {
19  _ai_char = ai_char;
20  _feeler = feeler_length;
21 }
22 
23 ObstacleAvoidance::~ObstacleAvoidance() {
24 }
25 
26 /////////////////////////////////////////////////////////////////////////////////
27 //
28 // Function : obstacle_detection
29 // Description : This function checks if an obstacle is near to the AICharacter and
30 // if an obstacle is detected returns true
31 
32 /////////////////////////////////////////////////////////////////////////////////
33 
35  // Calculate the volume of the AICharacter with respect to render
36  PT(BoundingVolume) np_bounds = _ai_char->get_node_path().get_bounds();
37  CPT(BoundingSphere) np_sphere = np_bounds->as_bounding_sphere();
38  LVecBase3 avoidance(0.0, 0.0, 0.0);
39  double distance = 0x7fff ;
40  double expanded_radius;
41  LVecBase3 to_obstacle;
42  LVecBase3 prev_avoidance;
43  for(unsigned int i = 0; i < _ai_char->_world->_obstacles.size(); ++i) {
44  PT(BoundingVolume) bounds = _ai_char->_world->_obstacles[i].get_bounds();
45  CPT(BoundingSphere) bsphere = bounds->as_bounding_sphere();
46  LVecBase3 near_obstacle = _ai_char->_world->_obstacles[i].get_pos() - _ai_char->get_node_path().get_pos();
47  // Check if it's the nearest obstacle, If so initialize as the nearest obstacle
48  if((near_obstacle.length() < distance) && (_ai_char->_world->_obstacles[i].get_pos() != _ai_char->get_node_path().get_pos())) {
49  _nearest_obstacle = _ai_char->_world->_obstacles[i];
50  distance = near_obstacle.length();
51  expanded_radius = bsphere->get_radius() + np_sphere->get_radius();
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  // If the nearest obstacle will collide with our AICharacter then send obstacle detection as true
62  if((_nearest_obstacle) && (perp.length() < expanded_radius - np_sphere->get_radius()) && (project.length() < feeler.length())) {
63  return true;
64  }
65  return false;
66 }
67 
68 /////////////////////////////////////////////////////////////////////////////////
69 //
70 // Function : obstacle_avoidance_activate
71 // Description : This function activates obstacle_avoidance if a obstacle
72 // is detected
73 
74 /////////////////////////////////////////////////////////////////////////////////
75 
77  if(obstacle_detection()) {
78  _ai_char->_steering->turn_off("obstacle_avoidance_activate");
79  _ai_char->_steering->turn_on("obstacle_avoidance");
80  }
81 }
82 
83 /////////////////////////////////////////////////////////////////////////////////
84 //
85 // Function : do_obstacle_avoidance
86 // Description : This function returns the force necessary by the AICharacter to
87 // avoid the nearest obstacle detected by obstacle_detection
88 // function
89 // NOTE : This assumes the obstacles are spherical
90 
91 /////////////////////////////////////////////////////////////////////////////////
92 
94  LVecBase3 offset = _ai_char->get_node_path().get_pos() - _nearest_obstacle.get_pos();
95  PT(BoundingVolume) bounds =_nearest_obstacle.get_bounds();
96  CPT(BoundingSphere) bsphere = bounds->as_bounding_sphere();
97  PT(BoundingVolume) np_bounds = _ai_char->get_node_path().get_bounds();
98  CPT(BoundingSphere) np_sphere = np_bounds->as_bounding_sphere();
99  double distance_needed = offset.length() - bsphere->get_radius() - np_sphere->get_radius();
100  if((obstacle_detection())) {
101  LVecBase3 direction = _ai_char->get_char_render().get_relative_vector(_ai_char->get_node_path(), LVector3::forward());
102  direction.normalize();
103  float forward_component = offset.dot(direction);
104  LVecBase3 projection = forward_component * direction;
105  LVecBase3 perpendicular_component = offset - projection;
106  double p = perpendicular_component.length();
107  perpendicular_component.normalize();
108  LVecBase3 avoidance = perpendicular_component;
109  // The more closer the obstacle, the more force it generates
110  avoidance = (avoidance * _ai_char->get_max_force() * _ai_char->_movt_force) / (p + 0.01);
111  return avoidance;
112  }
113  _ai_char->_steering->turn_on("obstacle_avoidance_activate");
114  _ai_char->_steering->turn_off("obstacle_avoidance");
115  return LVecBase3(0, 0, 0);
116 }
This is the base class for all three-component vectors and points.
Definition: lvecBase3.h:105
This defines a bounding sphere, consisting of a center and a radius.
float length_squared() const
Returns the square of the vector&#39;s length, cheap and easy.
Definition: lvecBase3.h:748
This is a three-component vector distance (as opposed to a three-component point, which represents a ...
Definition: lvector3.h:100
static LVector3f forward(CoordinateSystem cs=CS_default)
Returns the forward vector for the given coordinate system.
Definition: lvector3.h:565
bool obstacle_detection()
This function checks if an obstacle is near to the AICharacter and if an obstacle is detected returns...
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
float length() const
Returns the length of the vector, by the Pythagorean theorem.
Definition: lvecBase3.h:765
LPoint3 get_pos() const
Retrieves the translation component of the transform.
Definition: nodePath.cxx:1178
void turn_off(string ai_type)
This function turns off any aiBehavior which is passed as a string.
void turn_on(string ai_type)
This function turns on any aiBehavior which is passed as a string.
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:2212
void obstacle_avoidance_activate()
This function activates obstacle_avoidance if a obstacle is detected.
bool normalize()
Normalizes the vector in place.
Definition: lvecBase3.h:782
LVecBase3 do_obstacle_avoidance()
This function returns the force necessary by the AICharacter to avoid the nearest obstacle detected b...