Panda3D
collisionHandlerFluidPusher.cxx
1 // Filename: collisionHandlerFluidPusher.cxx
2 // Created by: drose (16Mar02)
3 //
4 ////////////////////////////////////////////////////////////////////
5 //
6 // PANDA 3D SOFTWARE
7 // Copyright (c) Carnegie Mellon University. All rights reserved.
8 //
9 // All use of this software is subject to the terms of the revised BSD
10 // license. You should have received a copy of this license along
11 // with this source code in a file named "LICENSE."
12 //
13 ////////////////////////////////////////////////////////////////////
14 
15 #include "collisionHandlerFluidPusher.h"
16 #include "collisionNode.h"
17 #include "collisionEntry.h"
18 #include "collisionPolygon.h"
19 #include "collisionSphere.h"
20 #include "config_collide.h"
21 #include "dcast.h"
22 
23 TypeHandle CollisionHandlerFluidPusher::_type_handle;
24 
25 ////////////////////////////////////////////////////////////////////
26 // Function: CollisionHandlerFluidPusher::Constructor
27 // Access: Public
28 // Description:
29 ////////////////////////////////////////////////////////////////////
30 CollisionHandlerFluidPusher::
31 CollisionHandlerFluidPusher() {
32  _wants_all_potential_collidees = true;
33 }
34 
35 ////////////////////////////////////////////////////////////////////
36 // Function: CollisionHandlerFluidPusher::add_entry
37 // Access: Public, Virtual
38 // Description: Called between a begin_group() .. end_group()
39 // sequence for each collision that is detected.
40 ////////////////////////////////////////////////////////////////////
43  nassertv(entry != (CollisionEntry *)NULL);
44  // skip over CollisionHandlerPhysical::add_entry, since it filters
45  // out collidees by orientation; our collider can change direction
46  // mid-frame, so it may collide with something that would have been
47  // filtered out
49 
50  // filter out non-tangibles
51  if (entry->get_from()->is_tangible() &&
52  (!entry->has_into() || entry->get_into()->is_tangible())) {
53 
54  _from_entries[entry->get_from_node_path()].push_back(entry);
55  if (entry->collided()) {
56  _has_contact = true;
57  }
58  }
59 }
60 
61 ////////////////////////////////////////////////////////////////////
62 // Function: CollisionHandlerFluidPusher::handle_entries
63 // Access: Protected, Virtual
64 // Description: Calculates a reasonable final position for a
65 // collider given a set of collidees
66 ////////////////////////////////////////////////////////////////////
67 bool CollisionHandlerFluidPusher::
68 handle_entries() {
69  /*
70  This pusher repeatedly calculates the first collision, calculates a new
71  trajectory based on that collision, and repeats until the original motion is
72  exhausted or the collider becomes "stuck". This solves the "acute collisions"
73  problem where colliders could bounce their way through to the other side
74  of a wall.
75 
76  Pseudocode:
77 
78  INPUTS
79  PosA = collider's previous position
80  PosB = collider's current position
81  M = movement vector (PosB - PosA)
82  BV = bounding sphere that includes collider at PosA and PosB
83  CS = 'collision set', all 'collidables' within BV (collision polys, tubes, etc)
84 
85  VARIABLES
86  N = movement vector since most recent collision (or start of frame)
87  SCS = 'sub collision set', all collidables that could still be collided with
88  C = single collider currently being collided with
89  PosX = new position given movement along N interrupted by collision with C
90 
91  OUTPUTS
92  final position is PosX
93 
94  1. N = M, SCS = CS, PosX = PosB
95  2. compute, using SCS and N, which collidable C is the first collision
96  3. if no collision found, DONE
97  4. if movement in direction M is now blocked, then
98  PosX = initial point of contact with C along N, DONE
99  5. calculate PosX (and new N) assuming that there will be no more collisions
100  6. remove C from SCS (assumes that you can't collide against a solid more than once per frame)
101  7. go to 2
102  */
103  bool okflag = true;
104 
105  // if all we got was potential collisions, don't bother
106  if (!_has_contact) {
107  return okflag;
108  }
109 
110  // for every fluid mover being pushed...
111  FromEntries::iterator fei;
112  for (fei = _from_entries.begin(); fei != _from_entries.end(); ++fei) {
113  NodePath from_node_path = fei->first;
114  Entries *orig_entries = &fei->second;
115 
116  Colliders::iterator ci;
117  ci = _colliders.find(from_node_path);
118  if (ci == _colliders.end()) {
119  // Hmm, someone added a CollisionNode to a traverser and gave
120  // it this CollisionHandler pointer--but they didn't tell us
121  // about the node.
122  collide_cat.error()
123  << "CollisionHandlerFluidPusher doesn't know about "
124  << from_node_path << ", disabling.\n";
125  okflag = false;
126  } else {
127  ColliderDef &def = (*ci).second;
128 
129  // we do our math in this node's space
130  NodePath wrt_node(*_root);
131 
132  // extract the collision entries into a vector that we can safely modify
133  Entries entries(*orig_entries);
134 
135  // this is the original position delta for the entire frame, before collision response
136  LVector3 M(from_node_path.get_pos_delta(wrt_node));
137  // this is used to track position deltas every time we collide against a solid
138  LVector3 N(M);
139 
140  const LPoint3 orig_pos(from_node_path.get_pos(wrt_node));
141  CPT(TransformState) prev_trans(from_node_path.get_prev_transform(wrt_node));
142  const LPoint3 orig_prev_pos(prev_trans->get_pos());
143 
144  // currently we only support spheres as the collider
145  const CollisionSphere *sphere;
146  DCAST_INTO_R(sphere, entries.front()->get_from(), false);
147 
148  from_node_path.set_pos(wrt_node, 0,0,0);
149  LPoint3 sphere_offset = (sphere->get_center() *
150  from_node_path.get_transform(wrt_node)->get_mat());
151  from_node_path.set_pos(wrt_node, orig_pos);
152 
153  // this will hold the final calculated position at each iteration
154  LPoint3 candidate_final_pos(orig_pos);
155  // this holds the position before reacting to collisions
156  LPoint3 uncollided_pos(candidate_final_pos);
157 
158  // unit vector facing back into original direction of motion
159  LVector3 reverse_vec(-M);
160  reverse_vec.normalize();
161 
162  // unit vector pointing out to the right relative to the direction of motion,
163  // looking into the direction of motion
164  const LVector3 right_unit(LVector3::up().cross(reverse_vec));
165 
166  // iterate until the mover runs out of movement or gets stuck
167  while (true) {
168  const CollisionEntry *C = 0;
169  // find the first (earliest) collision
170  Entries::const_iterator cei;
171  for (cei = entries.begin(); cei != entries.end(); ++cei) {
172  const CollisionEntry *entry = (*cei);
173  nassertr(entry != (CollisionEntry *)NULL, false);
174  if (entry->collided() && ((C == 0) || (entry->get_t() < C->get_t()))) {
175  nassertr(from_node_path == entry->get_from_node_path(), false);
176  C = entry;
177  }
178  }
179 
180  // if no collisions, we're done
181  if (C == 0) {
182  break;
183  }
184 
185  // move back to initial contact position
186  LPoint3 contact_pos;
187  LVector3 contact_normal;
188 
189  if (!C->get_all_contact_info(wrt_node, contact_pos, contact_normal)) {
190  collide_cat.warning()
191  << "Cannot shove on " << from_node_path << " for collision into "
192  << C->get_into_node_path() << "; no contact pos/normal information.\n";
193  break;
194  }
195  // calculate the position of the target node at the point of contact
196  contact_pos -= sphere_offset;
197 
198  uncollided_pos = candidate_final_pos;
199  candidate_final_pos = contact_pos;
200 
201  LVector3 proj_surface_normal(contact_normal);
202 
203  LVector3 norm_proj_surface_normal(proj_surface_normal);
204  norm_proj_surface_normal.normalize();
205 
206  LVector3 blocked_movement(uncollided_pos - contact_pos);
207 
208  PN_stdfloat push_magnitude(-blocked_movement.dot(proj_surface_normal));
209  if (push_magnitude < 0.0f) {
210  // don't ever push into plane
211  candidate_final_pos = contact_pos;
212  } else {
213  // calculate new position given that you collided with this thing
214  // project the final position onto the plane of the obstruction
215  candidate_final_pos = uncollided_pos + (norm_proj_surface_normal * push_magnitude);
216  }
217 
218  from_node_path.set_pos(wrt_node, candidate_final_pos);
219  CPT(TransformState) prev_trans(from_node_path.get_prev_transform(wrt_node));
220  prev_trans = prev_trans->set_pos(contact_pos);
221  from_node_path.set_prev_transform(wrt_node, prev_trans);
222 
223  {
224  const LPoint3 new_pos(from_node_path.get_pos(wrt_node));
225  CPT(TransformState) new_prev_trans(from_node_path.get_prev_transform(wrt_node));
226  const LPoint3 new_prev_pos(new_prev_trans->get_pos());
227  }
228 
229  // recalculate the position delta
230  N = from_node_path.get_pos_delta(wrt_node);
231 
232  // calculate new collisions given new movement vector
233  Entries::iterator ei;
234  Entries new_entries;
235  for (ei = entries.begin(); ei != entries.end(); ++ei) {
236  CollisionEntry *entry = (*ei);
237  nassertr(entry != (CollisionEntry *)NULL, false);
238  // skip the one we just collided against
239  if (entry != C) {
240  entry->_from_node_path = from_node_path;
241  entry->reset_collided();
242  PT(CollisionEntry) result = entry->get_from()->test_intersection(**ei);
243  if (result != (CollisionEntry *)NULL && result != (CollisionEntry *)0) {
244  new_entries.push_back(result);
245  }
246  }
247  }
248  entries.swap(new_entries);
249  }
250 
251  // put things back where they were
252  from_node_path.set_pos(wrt_node, orig_pos);
253  // restore the appropriate previous position
254  prev_trans = from_node_path.get_prev_transform(wrt_node);
255  prev_trans = prev_trans->set_pos(orig_prev_pos);
256  from_node_path.set_prev_transform(wrt_node, prev_trans);
257 
258  LVector3 net_shove(candidate_final_pos - orig_pos);
259  LVector3 force_normal(net_shove);
260  force_normal.normalize();
261 
262  // This is the part where the node actually gets moved:
263  def._target.set_pos(wrt_node, candidate_final_pos);
264 
265  // We call this to allow derived classes to do other
266  // fix-ups as they see fit:
267  apply_net_shove(def, net_shove, force_normal);
268  apply_linear_force(def, force_normal);
269  }
270  }
271 
272  return okflag;
273 }
NodePath get_into_node_path() const
Returns the NodePath that represents the specific CollisionNode or GeomNode instance that was collide...
bool collided() const
returns true if this represents an actual collision as opposed to a potential collision, needed for iterative collision resolution where path of collider changes mid-frame
This is a three-component vector distance (as opposed to a three-component point, which represents a ...
Definition: lvector3.h:100
This is a three-component point in space (as opposed to a three-component vector, which represents a ...
Definition: lpoint3.h:99
A spherical collision volume or object.
bool has_into() const
Returns true if the "into" solid is, in fact, a CollisionSolid, and its pointer is known (in which ca...
bool is_tangible() const
Returns whether the solid is considered &#39;tangible&#39; or not.
bool get_all_contact_info(const NodePath &space, LPoint3 &contact_pos, LVector3 &contact_normal) const
Simultaneously transforms the contact position and contact normal of the collision into the indicated...
LVector3 get_pos_delta() const
Returns the delta vector from this node&#39;s position in the previous frame (according to set_prev_trans...
Definition: nodePath.cxx:1198
const CollisionSolid * get_into() const
Returns the CollisionSolid pointer for the particular solid was collided into.
Defines a single collision event.
PN_stdfloat get_t() const
returns time value for this collision relative to other CollisionEntries
LPoint3 get_pos() const
Retrieves the translation component of the transform.
Definition: nodePath.cxx:1178
virtual void add_entry(CollisionEntry *entry)
Called between a begin_group() .
NodePath get_from_node_path() const
Returns the NodePath that represents the CollisionNode that contains the CollisionSolid that triggere...
TypeHandle is the identifier used to differentiate C++ class types.
Definition: typeHandle.h:85
const TransformState * get_transform(Thread *current_thread=Thread::get_current_thread()) const
Returns the complete transform object set on this node.
Definition: nodePath.cxx:925
bool normalize()
Normalizes the vector in place.
Definition: lvecBase3.h:783
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
Definition: nodePath.h:165
static LVector3f up(CoordinateSystem cs=CS_default)
Returns the up vector for the given coordinate system.
Definition: lvector3.h:541
virtual void add_entry(CollisionEntry *entry)
Called between a begin_group() .
void reset_collided()
prepare for another collision test
const TransformState * get_prev_transform(Thread *current_thread=Thread::get_current_thread()) const
Returns the transform that has been set as this node&#39;s "previous" position.
Definition: nodePath.cxx:1019
const CollisionSolid * get_from() const
Returns the CollisionSolid pointer for the particular solid that triggered this collision.