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 bool PQPCollision::updateBodyGeometry(Body*, bool)
00100 {
00101 DBGA("ERROR: Update body geometry not implemented in PQP collision");
00102 return false;
00103 }
00104
00105 void
00106 PQPCollision::removeBody(Body *body)
00107 {
00108 int id = getId(body);
00109 if (id < 0) {
00110 DBGA("PQPCollision: body not found");
00111 return;
00112 }
00113 int rc = mVc->DeleteObject(id);
00114 if (rc != VC_OK) {
00115 DBGA("PQPCollision: error deleting object");
00116 }
00117 mIds.erase( mIds.find(body) );
00118 mBodies.erase( mBodies.find(id) );
00119 }
00120
00121 void
00122 PQPCollision::cloneBody(Body *clone, const Body *original)
00123 {
00124 int originalId = getId(original);
00125 if (originalId < 0) {
00126 DBGA("PQPCollision: body not found");
00127 return;
00128 }
00129
00130 int id,rc;
00131
00132
00133 mMutex.lock();
00134 rc = mVc->NewClone(&id, originalId, getThreadId());
00135 mMutex.unlock();
00136
00137 if (rc != VC_OK) {
00138 DBGA("PQPCollision -- error creating clone: " << rc);
00139 return;
00140 }
00141 if ((rc = mVc->EndObject()) != VC_OK) {
00142 DBGA("PQPCollision -- error finishing clone: " << rc);
00143 return ;
00144 }
00145 mIds[clone] = id;
00146 mBodies[id] = clone;
00147 }
00148
00149 void
00150 PQPCollision::setBodyTransform(Body *body, const transf &t)
00151 {
00152 int id = getId(body);
00153 if (id < 0) {
00154 DBGA("PQPCollision: body not found");
00155 return;
00156 }
00157 double newTrans[4][4];
00158 t.toRowMajorMatrix(newTrans);
00159 mVc->UpdateTrans(id, newTrans);
00160 }
00161
00162 void
00163 PQPCollision::activateBody(const Body *body, bool active)
00164 {
00165 int id = getId(body);
00166 if (id < 0) {
00167 DBGA("PQPCollision: body not found");
00168 return;
00169 }
00170 if (active) {
00171 mVc->ActivateObject(id);
00172 } else {
00173 mVc->DeactivateObject(id);
00174 }
00175 }
00176
00177 void
00178 PQPCollision::activatePair(const Body *body1, const Body *body2, bool active)
00179 {
00180 int id1 = getId(body1);
00181 int id2 = getId(body2);
00182 if (id1 < 0 || id2 < 0) {
00183 DBGA("PQPCollision: body not found");
00184 return;
00185 }
00186 if (active) {
00187 mVc->ActivatePair(id1, id2);
00188 } else {
00189 mVc->DeactivatePair(id1, id2);
00190 }
00191 }
00192
00193 bool PQPCollision::isActive(const Body *body1, const Body *body2)
00194 {
00195 int id1 = getId(body1);
00196 if (id1 < 0) {
00197 DBGA("PQPCollision: body not found");
00198 return false;
00199 }
00200 if (!body2) {
00201 return mVc->isObjectActivated(id1);
00202 }
00203 int id2 = getId(body2);
00204 if (id2 < 0) {
00205 DBGA("PQPCollision: body not found");
00206 return false;
00207 }
00208 return mVc->isPairActivated(id1, id2);
00209 }
00210
00211 bool
00212 PQPCollision::getIdsFromList(const std::vector<Body*> *interestList, int **iList, int *iSize)
00213 {
00214 *iSize = 0;
00215 *iList = NULL;
00216 if ( interestList && !interestList->empty() ) {
00217 *iList = new int[ (int)interestList->size() ];
00218 std::vector<Body*>::const_iterator it;
00219 for (it = interestList->begin(); it!=interestList->end(); it++) {
00220 (*iList)[*iSize] = getId(*it);
00221 if ( (*iList)[*iSize]<0 ) {
00222 DBGA("PQPCollision: body not found");
00223 delete [] iList;
00224 return false;
00225 }
00226 (*iSize)++;
00227 }
00228 }
00229 return true;
00230 }
00231
00232 int
00233 PQPCollision::allCollisions(DetectionType type, CollisionReport *report,
00234 const std::vector<Body*> *interestList)
00235 {
00236
00237 int iSize; int *iList;
00238 if (!getIdsFromList(interestList, &iList, &iSize)) return 0;
00239
00240
00241 VCInternal::Query query;
00242 switch(type) {
00243 case FAST_COLLISION:
00244 query = VCInternal::FAST_COLLISION;
00245 break;
00246 case ALL_COLLISIONS:
00247 query = VCInternal::ALL_COLLISIONS;
00248 break;
00249 default:
00250 assert(0);
00251 }
00252
00253 VCReportType *result = NULL;
00254 int resultSize = 0;
00255 if (report) {
00256 resultSize = MAXCOLLISIONS;
00257 result = new VCReportType[resultSize];
00258 }
00259
00260 int numCols = mVc->AllCollisions(query, result, resultSize, iList, iSize, getThreadId());
00261
00262 if (numCols && result) {
00263 assert(report);
00264 assert(numCols < resultSize);
00265 for (int i=0; i<numCols; i++) {
00266 Body *b1 = getBody( result[i].id1 );
00267 assert(b1);
00268 Body *b2 = getBody( result[i].id2 );
00269 assert(b2);
00270 report->push_back( CollisionData(b1,b2) );
00271 }
00272 }
00273
00274 if (iList) delete [] iList;
00275 if (result) delete [] result;
00276 return numCols;
00277 }
00278
00279 void
00280 PQPCollision::convertContactReport(PQP_ContactResult *pqpReport, ContactReport *report)
00281 {
00282 ContactSetT::iterator it;
00283 for (it=pqpReport->contactSet.begin(); it!=pqpReport->contactSet.end(); it++) {
00284
00285 position b1p( (*it).b1_pos[0], (*it).b1_pos[1], (*it).b1_pos[2] );
00286 position b2p( (*it).b2_pos[0], (*it).b2_pos[1], (*it).b2_pos[2] );
00287
00288 vec3 b1n( (*it).b1_normal[0], (*it).b1_normal[1], (*it).b1_normal[2] );
00289 vec3 b2n( (*it).b2_normal[0], (*it).b2_normal[1], (*it).b2_normal[2] );
00290
00291 report->push_back( ContactData(b1p, b2p, b1n, b2n ) );
00292 }
00293 }
00294
00295 int
00296 PQPCollision::allContacts(CollisionReport *report, double threshold, const std::vector<Body*> *interestList)
00297 {
00298
00299 int iSize; int *iList;
00300 if (!getIdsFromList(interestList, &iList, &iSize)) return 0;
00301
00302
00303 int resultSize = MAXCOLLISIONS;
00304 PQP_ContactResult *result = new PQP_ContactResult[resultSize];
00305
00306
00307 int numContacts = mVc->AllContacts(result, threshold, iList, iSize, getThreadId());
00308
00309 for (int i=0; i<numContacts; i++) {
00310 Body *b1 = getBody( result[i].body1Id );
00311 assert(b1);
00312 Body *b2 = getBody( result[i].body2Id );
00313 assert(b2);
00314 report->push_back( CollisionData(b1,b2) );
00315
00316 convertContactReport( &result[i], &report->back().contacts );
00317
00318 compactContactSet(&report->back().contacts);
00319 }
00320
00321 delete [] result;
00322 return numContacts;
00323 }
00324
00325 int
00326 PQPCollision::contact(ContactReport *report, double threshold, const Body *body1, const Body *body2)
00327 {
00328 int id1 = getId(body1);
00329 int id2 = getId(body2);
00330 if (id1 < 0 || id2 < 0) {
00331 DBGA("PQPCollision: body not found");
00332 return 0;
00333 }
00334
00335 PQP_ContactResult result;
00336 int numContacts = mVc->Contact(id1, id2, threshold, result);
00337 convertContactReport( &result, report );
00338 compactContactSet(report);
00339 return numContacts;
00340 }
00341
00342 double
00343 PQPCollision::pointToBodyDistance(const Body *body1, position point,
00344 position &closestPoint, vec3 &closestNormal)
00345 {
00346 int id = getId(body1);
00347 if (id < 0) {
00348 DBGA("PQPCollision: body not found");
00349 return 0;
00350 }
00351 PQP_REAL pt[3], closest_pt[3], closest_normal[3];
00352 pt[0] = point.x(); pt[1] = point.y(); pt[2] = point.z();
00353 mVc->FindShortDist(pt, id, closest_pt, closest_normal, 0);
00354
00355 closestNormal.set(closest_normal[0], closest_normal[1], closest_normal[2]);
00356 closestPoint.set ( closest_pt[0], closest_pt[1], closest_pt[2]);
00357
00358 closestPoint = closestPoint * body1->getTran();
00359 closestNormal = normalise(closestNormal) * body1->getTran();
00360 vec3 dif = point - closestPoint;
00361 return dif.len();
00362 }
00363
00364 double
00365 PQPCollision::bodyToBodyDistance(const Body *body1, const Body *body2,
00366 position &p1, position &p2)
00367 {
00368 int id1 = getId(body1);
00369 int id2 = getId(body2);
00370 if (id1 < 0 || id2 < 0) {
00371 DBGA("PQPCollision: body not found");
00372 return 0;
00373 }
00374
00375 PQP_DistanceResult dRes;
00376 mVc->Dist(id1, id2, dRes, -1);
00377 p1.set( dRes.P1()[0], dRes.P1()[1] ,dRes.P1()[2] );
00378 p2.set( dRes.P2()[0], dRes.P2()[1], dRes.P2()[2] );
00379 return dRes.Distance();
00380 }
00381
00382 void
00383 PQPCollision::bodyRegion(const Body *body, position point, vec3 normal,
00384 double radius, Neighborhood *neighborhood)
00385 {
00386 int id = getId(body);
00387 if (id < 0) {
00388 DBGA("PQPCollision: body not found");
00389 return;
00390 }
00391 PQP_REAL pt[3], n[3];
00392 pt[0] = point.x(); pt[1] = point.y(); pt[2] = point.z();
00393 n[0] = normal.x(); n[1] = normal.y(); n[2] = normal.z();
00394 std::vector<PQP_Vec> ptList;
00395 mVc->FindRegion( id, pt, n, radius, &ptList );
00396
00397 for (int i=0; i<(int)ptList.size(); i++) {
00398 neighborhood->push_back( position( ptList[i].d ) );
00399 }
00400 }
00401
00402 void
00403 PQPCollision::getBoundingVolumes(const Body* body, int depth, std::vector<BoundingBox> *bvs)
00404 {
00405 int id = getId(body);
00406 if (id < 0) {
00407 DBGA("PQPCollision: body not found");
00408 return;
00409 }
00410 std::vector<BV*> pqp_bvs;
00411 mVc->getBvs(id, depth, &pqp_bvs);
00412 for (int i=0; i<(int)pqp_bvs.size(); i++) {
00413 vec3 size( pqp_bvs[i]->d[0], pqp_bvs[i]->d[1], pqp_bvs[i]->d[2] );
00414 vec3 loc( pqp_bvs[i]->To[0], pqp_bvs[i]->To[1], pqp_bvs[i]->To[2] );
00415
00416 vec3 v1( pqp_bvs[i]->R[0][0], pqp_bvs[i]->R[0][1], pqp_bvs[i]->R[0][2] );
00417 vec3 v2( pqp_bvs[i]->R[1][0], pqp_bvs[i]->R[1][1], pqp_bvs[i]->R[1][2] );
00418 vec3 v3( pqp_bvs[i]->R[2][0], pqp_bvs[i]->R[2][1], pqp_bvs[i]->R[2][2] );
00419 mat3 rot(v1,v2,v3);
00420 rot = rot.transpose();
00421
00422 transf tr(rot, loc);
00423 bvs->push_back( BoundingBox(tr, size) );
00424 delete pqp_bvs[i];
00425 }
00426 }
00427
00428 void
00429 PQPCollision::newThread()
00430 {
00431 if (mVc->newThread()!=VC_OK) {
00432 DBGA("Threading disabled in VCollide");
00433 return;
00434 }
00435 CollisionInterface::newThread();
00436 }
