00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include "PQPCollision.h"
00027
00028 #include "vcollide.h"
00029
00030 #include "body.h"
00031 #include "debug.h"
00032 #include "bBox.h"
00033 #include "triangle.h"
00034
00035 PQPCollision::PQPCollision()
00036 {
00037 mVc = new VCollide();
00038 }
00039
00040 PQPCollision::~PQPCollision()
00041 {
00042 delete mVc;
00043 }
00044
00045 bool
00046 PQPCollision::addBody(Body *body, bool ExpectEmpty)
00047 {
00048
00049 int id;
00050
00051
00052 mMutex.lock();
00053 int rc = mVc->NewObject(&id,getThreadId());
00054 mMutex.unlock();
00055
00056 if (rc != VC_OK) {
00057 DBGA("PQPCollision error in VCollide.NewObject(): " << rc );
00058 return false;
00059 }
00060
00061 if (id < 0) {
00062 DBGA("PQPCollision: VCollide returns negative id: " << id);
00063 return false;
00064 }
00065
00066 if ( mIds.find(body) != mIds.end() ) {
00067 DBGA("PQPCollision: body already present in id list");
00068 return false;
00069 }
00070 if (~ExpectEmpty){
00071
00072 std::vector<Triangle> triangles;
00073 body->getGeometryTriangles(&triangles);
00074
00075
00076 int triId = 0;
00077 double dp1[3], dp2[3], dp3[3];
00078 for (int i=0; i<(int)triangles.size(); i++) {
00079 triangles[i].v1.get(dp1);
00080 triangles[i].v2.get(dp2);
00081 triangles[i].v3.get(dp3);
00082 mVc->AddTri(dp1, dp2, dp3,triId++);
00083 }
00084 }
00085
00086
00087 if ((rc = mVc->EndObject(ExpectEmpty)) != VC_OK) {
00088 DBGA("PQPCollision error in VCollide.EndObject(): " << rc );
00089 return false;
00090 }
00091
00092
00093 mIds[body] = id;
00094 mBodies[id] = body;
00095
00096 return true;
00097 }
00098
00099 void
00100 PQPCollision::removeBody(Body *body)
00101 {
00102 int id = getId(body);
00103 if (id < 0) {
00104 DBGA("PQPCollision: body not found");
00105 return;
00106 }
00107 int rc = mVc->DeleteObject(id);
00108 if (rc != VC_OK) {
00109 DBGA("PQPCollision: error deleting object");
00110 }
00111 mIds.erase( mIds.find(body) );
00112 mBodies.erase( mBodies.find(id) );
00113 }
00114
00115 void
00116 PQPCollision::cloneBody(Body *clone, const Body *original)
00117 {
00118 int originalId = getId(original);
00119 if (originalId < 0) {
00120 DBGA("PQPCollision: body not found");
00121 return;
00122 }
00123
00124 int id,rc;
00125
00126
00127 mMutex.lock();
00128 rc = mVc->NewClone(&id, originalId, getThreadId());
00129 mMutex.unlock();
00130
00131 if (rc != VC_OK) {
00132 DBGA("PQPCollision -- error creating clone: " << rc);
00133 return;
00134 }
00135 if ((rc = mVc->EndObject()) != VC_OK) {
00136 DBGA("PQPCollision -- error finishing clone: " << rc);
00137 return ;
00138 }
00139 mIds[clone] = id;
00140 mBodies[id] = clone;
00141 }
00142
00143 void
00144 PQPCollision::setBodyTransform(Body *body, const transf &t)
00145 {
00146 int id = getId(body);
00147 if (id < 0) {
00148 DBGA("PQPCollision: body not found");
00149 return;
00150 }
00151 double newTrans[4][4];
00152 t.toRowMajorMatrix(newTrans);
00153 mVc->UpdateTrans(id, newTrans);
00154 }
00155
00156 void
00157 PQPCollision::activateBody(const Body *body, bool active)
00158 {
00159 int id = getId(body);
00160 if (id < 0) {
00161 DBGA("PQPCollision: body not found");
00162 return;
00163 }
00164 if (active) {
00165 mVc->ActivateObject(id);
00166 } else {
00167 mVc->DeactivateObject(id);
00168 }
00169 }
00170
00171 void
00172 PQPCollision::activatePair(const Body *body1, const Body *body2, bool active)
00173 {
00174 int id1 = getId(body1);
00175 int id2 = getId(body2);
00176 if (id1 < 0 || id2 < 0) {
00177 DBGA("PQPCollision: body not found");
00178 return;
00179 }
00180 if (active) {
00181 mVc->ActivatePair(id1, id2);
00182 } else {
00183 mVc->DeactivatePair(id1, id2);
00184 }
00185 }
00186
00187 bool PQPCollision::isActive(const Body *body1, const Body *body2)
00188 {
00189 int id1 = getId(body1);
00190 if (id1 < 0) {
00191 DBGA("PQPCollision: body not found");
00192 return false;
00193 }
00194 if (!body2) {
00195 return mVc->isObjectActivated(id1);
00196 }
00197 int id2 = getId(body2);
00198 if (id2 < 0) {
00199 DBGA("PQPCollision: body not found");
00200 return false;
00201 }
00202 return mVc->isPairActivated(id1, id2);
00203 }
00204
00205 bool
00206 PQPCollision::getIdsFromList(const std::vector<Body*> *interestList, int **iList, int *iSize)
00207 {
00208 *iSize = 0;
00209 *iList = NULL;
00210 if ( interestList && !interestList->empty() ) {
00211 *iList = new int[ (int)interestList->size() ];
00212 std::vector<Body*>::const_iterator it;
00213 for (it = interestList->begin(); it!=interestList->end(); it++) {
00214 (*iList)[*iSize] = getId(*it);
00215 if ( (*iList)[*iSize]<0 ) {
00216 DBGA("PQPCollision: body not found");
00217 delete [] iList;
00218 return false;
00219 }
00220 (*iSize)++;
00221 }
00222 }
00223 return true;
00224 }
00225
00226 int
00227 PQPCollision::allCollisions(DetectionType type, CollisionReport *report,
00228 const std::vector<Body*> *interestList)
00229 {
00230
00231 int iSize; int *iList;
00232 if (!getIdsFromList(interestList, &iList, &iSize)) return 0;
00233
00234
00235 VCInternal::Query query;
00236 switch(type) {
00237 case FAST_COLLISION:
00238 query = VCInternal::FAST_COLLISION;
00239 break;
00240 case ALL_COLLISIONS:
00241 query = VCInternal::ALL_COLLISIONS;
00242 break;
00243 default:
00244 assert(0);
00245 }
00246
00247 VCReportType *result = NULL;
00248 int resultSize = 0;
00249 if (report) {
00250 resultSize = MAXCOLLISIONS;
00251 result = new VCReportType[resultSize];
00252 }
00253
00254 int numCols = mVc->AllCollisions(query, result, resultSize, iList, iSize, getThreadId());
00255
00256 if (numCols && result) {
00257 assert(report);
00258 assert(numCols < resultSize);
00259 for (int i=0; i<numCols; i++) {
00260 Body *b1 = getBody( result[i].id1 );
00261 assert(b1);
00262 Body *b2 = getBody( result[i].id2 );
00263 assert(b2);
00264 report->push_back( CollisionData(b1,b2) );
00265 }
00266 }
00267
00268 if (iList) delete [] iList;
00269 if (result) delete [] result;
00270 return numCols;
00271 }
00272
00273 void
00274 PQPCollision::convertContactReport(PQP_ContactResult *pqpReport, ContactReport *report)
00275 {
00276 ContactSetT::iterator it;
00277 for (it=pqpReport->contactSet.begin(); it!=pqpReport->contactSet.end(); it++) {
00278
00279 position b1p( (*it).b1_pos[0], (*it).b1_pos[1], (*it).b1_pos[2] );
00280 position b2p( (*it).b2_pos[0], (*it).b2_pos[1], (*it).b2_pos[2] );
00281
00282 vec3 b1n( (*it).b1_normal[0], (*it).b1_normal[1], (*it).b1_normal[2] );
00283 vec3 b2n( (*it).b2_normal[0], (*it).b2_normal[1], (*it).b2_normal[2] );
00284
00285 report->push_back( ContactData(b1p, b2p, b1n, b2n ) );
00286 }
00287 }
00288
00289 int
00290 PQPCollision::allContacts(CollisionReport *report, double threshold, const std::vector<Body*> *interestList)
00291 {
00292
00293 int iSize; int *iList;
00294 if (!getIdsFromList(interestList, &iList, &iSize)) return 0;
00295
00296
00297 int resultSize = MAXCOLLISIONS;
00298 PQP_ContactResult *result = new PQP_ContactResult[resultSize];
00299
00300
00301 int numContacts = mVc->AllContacts(result, threshold, iList, iSize, getThreadId());
00302
00303 for (int i=0; i<numContacts; i++) {
00304 Body *b1 = getBody( result[i].body1Id );
00305 assert(b1);
00306 Body *b2 = getBody( result[i].body2Id );
00307 assert(b2);
00308 report->push_back( CollisionData(b1,b2) );
00309
00310 convertContactReport( &result[i], &report->back().contacts );
00311
00312 compactContactSet(&report->back().contacts);
00313 }
00314
00315 delete [] result;
00316 return numContacts;
00317 }
00318
00319 int
00320 PQPCollision::contact(ContactReport *report, double threshold, const Body *body1, const Body *body2)
00321 {
00322 int id1 = getId(body1);
00323 int id2 = getId(body2);
00324 if (id1 < 0 || id2 < 0) {
00325 DBGA("PQPCollision: body not found");
00326 return 0;
00327 }
00328
00329 PQP_ContactResult result;
00330 int numContacts = mVc->Contact(id1, id2, threshold, result);
00331 convertContactReport( &result, report );
00332 compactContactSet(report);
00333 return numContacts;
00334 }
00335
00336 double
00337 PQPCollision::pointToBodyDistance(const Body *body1, position point,
00338 position &closestPoint, vec3 &closestNormal)
00339 {
00340 int id = getId(body1);
00341 if (id < 0) {
00342 DBGA("PQPCollision: body not found");
00343 return 0;
00344 }
00345 PQP_REAL pt[3], closest_pt[3], closest_normal[3];
00346 pt[0] = point.x(); pt[1] = point.y(); pt[2] = point.z();
00347 mVc->FindShortDist(pt, id, closest_pt, closest_normal, 0);
00348
00349 closestNormal.set(closest_normal[0], closest_normal[1], closest_normal[2]);
00350 closestPoint.set ( closest_pt[0], closest_pt[1], closest_pt[2]);
00351
00352 closestPoint = closestPoint * body1->getTran();
00353 closestNormal = normalise(closestNormal) * body1->getTran();
00354 vec3 dif = point - closestPoint;
00355 return dif.len();
00356 }
00357
00358 double
00359 PQPCollision::bodyToBodyDistance(const Body *body1, const Body *body2,
00360 position &p1, position &p2)
00361 {
00362 int id1 = getId(body1);
00363 int id2 = getId(body2);
00364 if (id1 < 0 || id2 < 0) {
00365 DBGA("PQPCollision: body not found");
00366 return 0;
00367 }
00368
00369 PQP_DistanceResult dRes;
00370 mVc->Dist(id1, id2, dRes, -1);
00371 p1.set( dRes.P1()[0], dRes.P1()[1] ,dRes.P1()[2] );
00372 p2.set( dRes.P2()[0], dRes.P2()[1], dRes.P2()[2] );
00373 return dRes.Distance();
00374 }
00375
00376 void
00377 PQPCollision::bodyRegion(const Body *body, position point, vec3 normal,
00378 double radius, Neighborhood *neighborhood)
00379 {
00380 int id = getId(body);
00381 if (id < 0) {
00382 DBGA("PQPCollision: body not found");
00383 return;
00384 }
00385 PQP_REAL pt[3], n[3];
00386 pt[0] = point.x(); pt[1] = point.y(); pt[2] = point.z();
00387 n[0] = normal.x(); n[1] = normal.y(); n[2] = normal.z();
00388 std::vector<PQP_Vec> ptList;
00389 mVc->FindRegion( id, pt, n, radius, &ptList );
00390
00391 for (int i=0; i<(int)ptList.size(); i++) {
00392 neighborhood->push_back( position( ptList[i].d ) );
00393 }
00394 }
00395
00396 void
00397 PQPCollision::getBoundingVolumes(const Body* body, int depth, std::vector<BoundingBox> *bvs)
00398 {
00399 int id = getId(body);
00400 if (id < 0) {
00401 DBGA("PQPCollision: body not found");
00402 return;
00403 }
00404 std::vector<BV*> pqp_bvs;
00405 mVc->getBvs(id, depth, &pqp_bvs);
00406 for (int i=0; i<(int)pqp_bvs.size(); i++) {
00407 vec3 size( pqp_bvs[i]->d[0], pqp_bvs[i]->d[1], pqp_bvs[i]->d[2] );
00408 vec3 loc( pqp_bvs[i]->To[0], pqp_bvs[i]->To[1], pqp_bvs[i]->To[2] );
00409
00410 vec3 v1( pqp_bvs[i]->R[0][0], pqp_bvs[i]->R[0][1], pqp_bvs[i]->R[0][2] );
00411 vec3 v2( pqp_bvs[i]->R[1][0], pqp_bvs[i]->R[1][1], pqp_bvs[i]->R[1][2] );
00412 vec3 v3( pqp_bvs[i]->R[2][0], pqp_bvs[i]->R[2][1], pqp_bvs[i]->R[2][2] );
00413 mat3 rot(v1,v2,v3);
00414 rot = rot.transpose();
00415
00416 transf tr(rot, loc);
00417 bvs->push_back( BoundingBox(tr, size) );
00418 delete pqp_bvs[i];
00419 }
00420 }
00421
00422 void
00423 PQPCollision::newThread()
00424 {
00425 if (mVc->newThread()!=VC_OK) {
00426 DBGA("Threading disabled in VCollide");
00427 return;
00428 }
00429 CollisionInterface::newThread();
00430 }
