00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "collisionHandlerFluidPusher.h"
00016 #include "collisionNode.h"
00017 #include "collisionEntry.h"
00018 #include "collisionPolygon.h"
00019 #include "config_collide.h"
00020 #include "dcast.h"
00021
00022 TypeHandle CollisionHandlerFluidPusher::_type_handle;
00023
00024
00025
00026
00027
00028
00029 CollisionHandlerFluidPusher::
00030 CollisionHandlerFluidPusher() {
00031 _wants_all_potential_collidees = true;
00032 }
00033
00034
00035
00036
00037
00038
00039
00040 void CollisionHandlerFluidPusher::
00041 add_entry(CollisionEntry *entry) {
00042 nassertv(entry != (CollisionEntry *)NULL);
00043
00044
00045
00046
00047 CollisionHandlerEvent::add_entry(entry);
00048
00049
00050 if (entry->get_from()->is_tangible() &&
00051 (!entry->has_into() || entry->get_into()->is_tangible())) {
00052
00053 _from_entries[entry->get_from_node_path()].push_back(entry);
00054 if (entry->collided()) {
00055 _has_contact = true;
00056 }
00057 }
00058 }
00059
00060
00061
00062
00063
00064
00065
00066 bool CollisionHandlerFluidPusher::
00067 handle_entries() {
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102 bool okflag = true;
00103
00104
00105 if (!_has_contact) {
00106 return okflag;
00107 }
00108
00109
00110 FromEntries::iterator fei;
00111 for (fei = _from_entries.begin(); fei != _from_entries.end(); ++fei) {
00112 NodePath from_node_path = fei->first;
00113 Entries *orig_entries = &fei->second;
00114
00115 Colliders::iterator ci;
00116 ci = _colliders.find(from_node_path);
00117 if (ci == _colliders.end()) {
00118
00119
00120
00121 collide_cat.error()
00122 << "CollisionHandlerFluidPusher doesn't know about "
00123 << from_node_path << ", disabling.\n";
00124 okflag = false;
00125 } else {
00126 ColliderDef &def = (*ci).second;
00127
00128
00129 NodePath wrt_node(*_root);
00130
00131
00132 Entries entries(*orig_entries);
00133
00134
00135 LVector3 M(from_node_path.get_pos_delta(wrt_node));
00136
00137 LVector3 N(M);
00138
00139 const LPoint3 orig_pos(from_node_path.get_pos(wrt_node));
00140 CPT(TransformState) prev_trans(from_node_path.get_prev_transform(wrt_node));
00141 const LPoint3 orig_prev_pos(prev_trans->get_pos());
00142
00143
00144 const CollisionSphere *sphere;
00145 DCAST_INTO_R(sphere, entries.front()->get_from(), 0);
00146
00147 from_node_path.set_pos(wrt_node, 0,0,0);
00148 LPoint3 sphere_offset = (sphere->get_center() *
00149 from_node_path.get_transform(wrt_node)->get_mat());
00150 from_node_path.set_pos(wrt_node, orig_pos);
00151
00152
00153 LPoint3 candidate_final_pos(orig_pos);
00154
00155 LPoint3 uncollided_pos(candidate_final_pos);
00156
00157
00158 LVector3 reverse_vec(-M);
00159 reverse_vec.normalize();
00160
00161
00162
00163 const LVector3 right_unit(LVector3::up().cross(reverse_vec));
00164
00165
00166 while (true) {
00167 const CollisionEntry *C = 0;
00168
00169 Entries::const_iterator cei;
00170 for (cei = entries.begin(); cei != entries.end(); ++cei) {
00171 const CollisionEntry *entry = (*cei);
00172 nassertr(entry != (CollisionEntry *)NULL, false);
00173 if (entry->collided() && ((C == 0) || (entry->get_t() < C->get_t()))) {
00174 nassertr(from_node_path == entry->get_from_node_path(), false);
00175 C = entry;
00176 }
00177 }
00178
00179
00180 if (C == 0) {
00181 break;
00182 }
00183
00184
00185 LPoint3 contact_pos;
00186 LVector3 contact_normal;
00187
00188 if (!C->get_all_contact_info(wrt_node, contact_pos, contact_normal)) {
00189 collide_cat.warning()
00190 << "Cannot shove on " << from_node_path << " for collision into "
00191 << C->get_into_node_path() << "; no contact pos/normal information.\n";
00192 break;
00193 }
00194
00195 contact_pos -= sphere_offset;
00196
00197 uncollided_pos = candidate_final_pos;
00198 candidate_final_pos = contact_pos;
00199
00200 LVector3 proj_surface_normal(contact_normal);
00201
00202 LVector3 norm_proj_surface_normal(proj_surface_normal);
00203 norm_proj_surface_normal.normalize();
00204
00205 LVector3 blocked_movement(uncollided_pos - contact_pos);
00206
00207 PN_stdfloat push_magnitude(-blocked_movement.dot(proj_surface_normal));
00208 if (push_magnitude < 0.0f) {
00209
00210 candidate_final_pos = contact_pos;
00211 } else {
00212
00213
00214 candidate_final_pos = uncollided_pos + (norm_proj_surface_normal * push_magnitude);
00215 }
00216
00217 from_node_path.set_pos(wrt_node, candidate_final_pos);
00218 CPT(TransformState) prev_trans(from_node_path.get_prev_transform(wrt_node));
00219 prev_trans = prev_trans->set_pos(contact_pos);
00220 from_node_path.set_prev_transform(wrt_node, prev_trans);
00221
00222 {
00223 const LPoint3 new_pos(from_node_path.get_pos(wrt_node));
00224 CPT(TransformState) new_prev_trans(from_node_path.get_prev_transform(wrt_node));
00225 const LPoint3 new_prev_pos(new_prev_trans->get_pos());
00226 }
00227
00228
00229 N = from_node_path.get_pos_delta(wrt_node);
00230
00231
00232 Entries::iterator ei;
00233 Entries new_entries;
00234 for (ei = entries.begin(); ei != entries.end(); ++ei) {
00235 CollisionEntry *entry = (*ei);
00236 nassertr(entry != (CollisionEntry *)NULL, false);
00237
00238 if (entry != C) {
00239 entry->_from_node_path = from_node_path;
00240 entry->reset_collided();
00241 PT(CollisionEntry) result = entry->get_from()->test_intersection(**ei);
00242 if (result != (CollisionEntry *)NULL && result != (CollisionEntry *)0) {
00243 new_entries.push_back(result);
00244 }
00245 }
00246 }
00247 entries.swap(new_entries);
00248 }
00249
00250
00251 from_node_path.set_pos(wrt_node, orig_pos);
00252
00253 prev_trans = from_node_path.get_prev_transform(wrt_node);
00254 prev_trans = prev_trans->set_pos(orig_prev_pos);
00255 from_node_path.set_prev_transform(wrt_node, prev_trans);
00256
00257 LVector3 net_shove(candidate_final_pos - orig_pos);
00258 LVector3 force_normal(net_shove);
00259 force_normal.normalize();
00260
00261
00262 def._target.set_pos(wrt_node, candidate_final_pos);
00263
00264
00265
00266 apply_net_shove(def, net_shove, force_normal);
00267 apply_linear_force(def, force_normal);
00268 }
00269 }
00270
00271 return okflag;
00272 }