00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "collisionHandlerPusher.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 CollisionHandlerPusher::_type_handle;
00023
00024
00025
00026
00027
00028
00029
00030
00031 class ShoveData {
00032 public:
00033 LVector3 _vector;
00034 PN_stdfloat _length;
00035 bool _valid;
00036 CollisionEntry *_entry;
00037 };
00038
00039
00040
00041
00042
00043
00044 CollisionHandlerPusher::
00045 CollisionHandlerPusher() {
00046 _horizontal = pushers_horizontal;
00047 }
00048
00049
00050
00051
00052
00053
00054 CollisionHandlerPusher::
00055 ~CollisionHandlerPusher() {
00056 }
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069 bool CollisionHandlerPusher::
00070 handle_entries() {
00071 bool okflag = true;
00072
00073 FromEntries::const_iterator fi;
00074 for (fi = _from_entries.begin(); fi != _from_entries.end(); ++fi) {
00075 const NodePath &from_node_path = (*fi).first;
00076 const Entries &entries = (*fi).second;
00077
00078 Colliders::iterator ci;
00079 ci = _colliders.find(from_node_path);
00080 if (ci == _colliders.end()) {
00081
00082
00083
00084 collide_cat.error()
00085 << "CollisionHandlerPusher doesn't know about "
00086 << from_node_path << ", disabling.\n";
00087 okflag = false;
00088 } else {
00089 ColliderDef &def = (*ci).second;
00090 {
00091
00092
00093
00094
00095
00096
00097
00098
00099 typedef epvector<ShoveData> Shoves;
00100 Shoves shoves;
00101
00102 Entries::const_iterator ei;
00103 for (ei = entries.begin(); ei != entries.end(); ++ei) {
00104 CollisionEntry *entry = (*ei);
00105 nassertr(entry != (CollisionEntry *)NULL, false);
00106 nassertr(from_node_path == entry->get_from_node_path(), false);
00107
00108 LPoint3 surface_point;
00109 LVector3 normal;
00110 LPoint3 interior_point;
00111
00112 if (!entry->get_all(def._target, surface_point, normal, interior_point)) {
00113 #ifndef NDEBUG
00114 if (collide_cat.is_debug()) {
00115 collide_cat.debug()
00116 << "Cannot shove on " << from_node_path << " for collision into "
00117 << entry->get_into_node_path() << "; no normal/depth information.\n";
00118 }
00119 #endif
00120 } else {
00121
00122 if (!surface_point.almost_equal(interior_point)) {
00123 if (_horizontal) {
00124 normal[2] = 0.0f;
00125 }
00126
00127
00128
00129 normal.normalize();
00130
00131 ShoveData sd;
00132 sd._vector = normal;
00133 sd._length = (surface_point - interior_point).length();
00134 sd._valid = true;
00135 sd._entry = entry;
00136
00137 #ifndef NDEBUG
00138 if (collide_cat.is_debug()) {
00139 collide_cat.debug()
00140 << "Shove on " << from_node_path << " from "
00141 << entry->get_into_node_path() << ": " << sd._vector
00142 << " times " << sd._length << "\n";
00143 }
00144 #endif
00145
00146 shoves.push_back(sd);
00147 }
00148 }
00149 }
00150
00151 if (!shoves.empty()) {
00152
00153
00154
00155
00156
00157
00158 Shoves::iterator si;
00159 for (si = shoves.begin(); si != shoves.end(); ++si) {
00160 ShoveData &sd = (*si);
00161 Shoves::iterator sj;
00162 for (sj = shoves.begin(); sj != si; ++sj) {
00163 ShoveData &sd2 = (*sj);
00164 if (sd2._valid) {
00165 PN_stdfloat d = sd._vector.dot(sd2._vector);
00166 if (collide_cat.is_debug()) {
00167 collide_cat.debug()
00168 << "Considering dot product " << d << "\n";
00169 }
00170
00171 if (d > 0.9) {
00172
00173
00174 if (sd2._length < sd._length) {
00175 sd2._valid = false;
00176 } else {
00177 sd._valid = false;
00178 }
00179 } else {
00180
00181
00182
00183
00184 const CollisionSolid *s1 = sd._entry->get_into();
00185 const CollisionSolid *s2 = sd2._entry->get_into();
00186 if (s1 != (CollisionSolid *)NULL &&
00187 s2 != (CollisionSolid *)NULL &&
00188 s1->is_exact_type(CollisionPolygon::get_class_type()) &&
00189 s2->is_exact_type(CollisionPolygon::get_class_type()) &&
00190 sd._entry->get_into_node_path() ==
00191 sd2._entry->get_into_node_path()) {
00192 const CollisionPolygon *p1 = DCAST(CollisionPolygon, s1);
00193 const CollisionPolygon *p2 = DCAST(CollisionPolygon, s2);
00194 if (p1->dist_to_plane(p2->get_collision_origin()) < 0 &&
00195 p2->dist_to_plane(p1->get_collision_origin()) < 0) {
00196
00197
00198
00199
00200
00201 if (collide_cat.is_debug()) {
00202 collide_cat.debug()
00203 << "Discarding shove from convex corner.\n";
00204 }
00205
00206
00207
00208
00209
00210
00211
00212
00213 if (sd2._length < sd._length) {
00214 sd._valid = false;
00215 } else {
00216 sd2._valid = false;
00217 }
00218 }
00219 }
00220 }
00221 }
00222 }
00223 }
00224
00225
00226 LVector3 net_shove(0.0f, 0.0f, 0.0f);
00227 LVector3 force_normal(0.0f, 0.0f, 0.0f);
00228 for (si = shoves.begin(); si != shoves.end(); ++si) {
00229 const ShoveData &sd = (*si);
00230 if (sd._valid) {
00231 net_shove += sd._vector * sd._length;
00232 force_normal += sd._vector;
00233 }
00234 }
00235
00236 #ifndef NDEBUG
00237 if (collide_cat.is_debug()) {
00238 collide_cat.debug()
00239 << "Net shove on " << from_node_path << " is: "
00240 << net_shove << "\n";
00241 }
00242 #endif
00243
00244
00245 CPT(TransformState) trans = def._target.get_transform();
00246 LVecBase3 pos = trans->get_pos();
00247 pos += net_shove * trans->get_mat();
00248 def._target.set_transform(trans->set_pos(pos));
00249 def.updated_transform();
00250
00251
00252
00253 apply_net_shove(def, net_shove, force_normal);
00254 apply_linear_force(def, force_normal);
00255 }
00256 }
00257 }
00258 }
00259
00260 return okflag;
00261 }
00262
00263
00264
00265
00266
00267
00268
00269 void CollisionHandlerPusher::
00270 apply_net_shove(ColliderDef &def, const LVector3 &net_shove,
00271 const LVector3 &force_normal) {
00272 }
00273
00274
00275
00276
00277
00278
00279
00280 void CollisionHandlerPusher::
00281 apply_linear_force(ColliderDef &def, const LVector3 &force_normal) {
00282 }