00001
00002
00003
00004
00005
00006
00007
00008
00014 #include <iostream>
00015 #include <math.h>
00016 #include "ColdetModelPair.h"
00017 #include "ColdetModelSharedDataSet.h"
00018 #include "CollisionPairInserter.h"
00019 #include "Opcode/Opcode.h"
00020 #include "SSVTreeCollider.h"
00021
00022 using namespace hrp;
00023
00024
00025 ColdetModelPair::ColdetModelPair()
00026 {
00027 collisionPairInserter = new CollisionPairInserter;
00028 }
00029
00030
00031 ColdetModelPair::ColdetModelPair(ColdetModelPtr model0, ColdetModelPtr model1,
00032 double tolerance)
00033 {
00034 collisionPairInserter = new CollisionPairInserter;
00035 set(model0, model1);
00036 tolerance_ = tolerance;
00037 }
00038
00039
00040 ColdetModelPair::ColdetModelPair(const ColdetModelPair& org)
00041 {
00042 collisionPairInserter = new CollisionPairInserter;
00043 set(org.models[0], org.models[1]);
00044 tolerance_ = org.tolerance_;
00045 }
00046
00047
00048 ColdetModelPair::~ColdetModelPair()
00049 {
00050 delete collisionPairInserter;
00051 }
00052
00053
00054 void ColdetModelPair::set(ColdetModelPtr model0, ColdetModelPtr model1)
00055 {
00056 models[0] = model0;
00057 models[1] = model1;
00058
00059
00060 if(model0 && model1)
00061 collisionPairInserter->set(model1->dataSet, model0->dataSet);
00062 }
00063
00064
00065 std::vector<collision_data>& ColdetModelPair::detectCollisionsSub(bool detectAllContacts)
00066 {
00067 collisionPairInserter->clear();
00068
00069 int pt0 = models[0]->getPrimitiveType();
00070 int pt1 = models[1]->getPrimitiveType();
00071 bool detected;
00072 bool detectPlaneSphereCollisions(bool detectAllContacts);
00073
00074 if (( pt0 == ColdetModel::SP_PLANE && pt1 == ColdetModel::SP_CYLINDER)
00075 || (pt1 == ColdetModel::SP_PLANE && pt0 == ColdetModel::SP_CYLINDER)){
00076 detected = detectPlaneCylinderCollisions(detectAllContacts);
00077 }
00078 else if (pt0 == ColdetModel::SP_PLANE || pt1 == ColdetModel::SP_PLANE){
00079 detected = detectPlaneMeshCollisions(detectAllContacts);
00080 }
00081 else if (pt0 == ColdetModel::SP_SPHERE && pt1 == ColdetModel::SP_SPHERE) {
00082 detected = detectSphereSphereCollisions(detectAllContacts);
00083 }
00084
00085 else if (pt0 == ColdetModel::SP_SPHERE || pt1 == ColdetModel::SP_SPHERE) {
00086 detected = detectSphereMeshCollisions(detectAllContacts);
00087 }
00088 else {
00089 detected = detectMeshMeshCollisions(detectAllContacts);
00090 }
00091
00092 if(!detected){
00093 collisionPairInserter->clear();
00094 }
00095
00096 return collisionPairInserter->collisions();
00097 }
00098
00099
00100 bool ColdetModelPair::detectPlaneMeshCollisions(bool detectAllContacts)
00101 {
00102 bool result = false;
00103
00104 ColdetModelPtr plane, mesh;
00105 bool reversed=false;
00106 if (models[0]->getPrimitiveType() == ColdetModel::SP_PLANE){
00107 plane = models[0];
00108 mesh = models[1];
00109 }
00110 if (models[1]->getPrimitiveType() == ColdetModel::SP_PLANE){
00111 plane = models[1];
00112 mesh = models[0];
00113 reversed = true;
00114 }
00115 if (!plane || !mesh || !mesh->dataSet->model.GetMeshInterface()) return false;
00116
00117
00118 PlanesCollider PC;
00119 if(!detectAllContacts) PC.SetFirstContact(true);
00120 PC.setCollisionPairInserter(collisionPairInserter);
00121 IceMaths::Matrix4x4 mTrans = *(mesh->transform);
00122 for(udword i=0; i<3; i++){
00123 for(udword j=0; j<3; j++){
00124 collisionPairInserter->CD_Rot1(i,j) = mTrans[j][i];
00125 }
00126 collisionPairInserter->CD_Trans1[i] = mTrans[3][i];
00127 }
00128 collisionPairInserter->CD_s1 = 1.0;
00129
00130 PlanesCache Cache;
00131 IceMaths::Matrix4x4 pTrans = (*(plane->pTransform)) * (*(plane->transform));
00132 IceMaths::Point p, nLocal(0,0,1), n;
00133 IceMaths::TransformPoint3x3(n, nLocal, pTrans);
00134 pTrans.GetTrans(p);
00135 Plane Planes[] = {Plane(p, n)};
00136 bool IsOk = PC.Collide(Cache, Planes, 1, mesh->dataSet->model,
00137 mesh->transform);
00138 if (!IsOk){
00139 std::cerr << "PlanesCollider::Collide() failed" << std::endl;
00140 }else{
00141 result = PC.GetContactStatus();
00142 }
00143 if (reversed){
00144 std::vector<collision_data>& cdata
00145 = collisionPairInserter->collisions();
00146 for (size_t i=0; i<cdata.size(); i++){
00147 cdata[i].n_vector *= -1;
00148 }
00149 }
00150
00151 return result;
00152 }
00153
00154 bool ColdetModelPair::detectMeshMeshCollisions(bool detectAllContacts)
00155 {
00156 bool result = false;
00157
00158 if(models[0]->isValid() && models[1]->isValid()){
00159
00160 Opcode::BVTCache colCache;
00161
00162
00163
00164 colCache.Model0 = &models[1]->dataSet->model;
00165 colCache.Model1 = &models[0]->dataSet->model;
00166
00167 if(colCache.Model0->HasSingleNode() || colCache.Model1->HasSingleNode())
00168 return result;
00169
00170 Opcode::AABBTreeCollider collider;
00171 collider.setCollisionPairInserter(collisionPairInserter);
00172
00173 if(!detectAllContacts){
00174 collider.SetFirstContact(true);
00175 }
00176
00177 bool isOk = collider.Collide(colCache, models[1]->transform, models[0]->transform);
00178
00179 if (!isOk)
00180 std::cerr << "AABBTreeCollider::Collide() failed" << std::endl;
00181
00182 result = collider.GetContactStatus();
00183
00184 boxTestsCount = collider.GetNbBVBVTests();
00185 triTestsCount = collider.GetNbPrimPrimTests();
00186 }
00187
00188 return result;
00189 }
00190
00191 bool ColdetModelPair::detectSphereSphereCollisions(bool detectAllContacts) {
00192
00193 bool result = false;
00194 int sign = 1;
00195
00196 if (models[0]->isValid() && models[1]->isValid()) {
00197
00198 ColdetModelPtr sphereA, sphereB;
00199 sphereA = models[0];
00200 sphereB = models[1];
00201
00202 IceMaths::Matrix4x4 sATrans = (*(sphereA->pTransform)) * (*(sphereA->transform));
00203 IceMaths::Matrix4x4 sBTrans = (*(sphereB->pTransform)) * (*(sphereB->transform));
00204
00205 float radiusA, radiusB;
00206 sphereA->getPrimitiveParam(0, radiusA);
00207 sphereB->getPrimitiveParam(0, radiusB);
00208
00209 IceMaths::Point centerA = sATrans.GetTrans();
00210 IceMaths::Point centerB = sBTrans.GetTrans();
00211
00212 IceMaths::Point D = centerB - centerA;
00213
00214 float depth = radiusA + radiusB - D.Magnitude();
00215
00216 if (D.Magnitude() <= (radiusA + radiusB)) {
00217
00218 result = true;
00219
00220 float x = (pow(D.Magnitude(), 2) + pow(radiusA, 2) - pow(radiusB, 2)) / (2 * D.Magnitude());
00221
00222 IceMaths::Point n = D / D.Magnitude();
00223
00224 IceMaths::Point q = centerA + n * x;
00225
00226 std::vector<collision_data>& cdata = collisionPairInserter->collisions();
00227 cdata.clear();
00228
00229 collision_data col;
00230 col.depth = depth;
00231 col.num_of_i_points = 1;
00232 col.i_point_new[0] = 1;
00233 col.i_point_new[1] = 0;
00234 col.i_point_new[2] = 0;
00235 col.i_point_new[3] = 0;
00236 col.n_vector[0] = sign * n.x;
00237 col.n_vector[1] = sign * n.y;
00238 col.n_vector[2] = sign * n.z;
00239 col.i_points[0][0] = q.x;
00240 col.i_points[0][1] = q.y;
00241 col.i_points[0][2] = q.z;
00242 cdata.push_back(col);
00243 }
00244 }
00245
00246 return result;
00247 }
00248
00249 bool ColdetModelPair::detectSphereMeshCollisions(bool detectAllContacts) {
00250
00251 bool result = false;
00252 int sign = 1;
00253
00254 if (models[0]->isValid() && models[1]->isValid()) {
00255
00256 ColdetModelPtr sphere, mesh;
00257
00258 if (models[0]->getPrimitiveType() == ColdetModel::SP_SPHERE) {
00259 sphere = models[0];
00260 mesh = models[1];
00261 sign = -1;
00262 }
00263 else if (models[1]->getPrimitiveType() == ColdetModel::SP_SPHERE) {
00264 sphere = models[1];
00265 mesh = models[0];
00266 }
00267
00268 if (!sphere || !mesh)
00269 return false;
00270
00271 IceMaths::Matrix4x4 sTrans = (*(sphere->pTransform)) * (*(sphere->transform));
00272
00273 float radius;
00274 sphere->getPrimitiveParam(0, radius);
00275
00276 IceMaths::Sphere sphere_def(IceMaths::Point(0, 0, 0), radius);
00277
00278 Opcode::SphereCache colCache;
00279
00280 Opcode::SphereCollider collider;
00281
00282 if (!detectAllContacts) {
00283 collider.SetFirstContact(true);
00284 }
00285
00286 bool isOk = collider.Collide(colCache, sphere_def, mesh->dataSet->model, &sTrans, mesh->transform);
00287
00288 if (isOk) {
00289
00290 if (collider.GetContactStatus()) {
00291
00292 int TouchedPrimCount = collider.GetNbTouchedPrimitives();
00293 const udword* TouchedPrim = collider.GetTouchedPrimitives();
00294
00295 if (TouchedPrimCount) {
00296
00297 result = true;
00298
00299 std::vector< std::vector<IceMaths::Point> > triangle(TouchedPrimCount);
00300 std::vector<IceMaths::Plane> face(TouchedPrimCount);
00301
00302 std::vector<float> depth(TouchedPrimCount);
00303
00304 std::vector<IceMaths::Point> q(TouchedPrimCount);
00305 std::vector<float> A(TouchedPrimCount);
00306
00307 IceMaths::Matrix4x4 sTransInv;
00308 IceMaths::InvertPRMatrix(sTransInv, sTrans);
00309
00310 std::vector<collision_data>& cdata = collisionPairInserter->collisions();
00311 cdata.clear();
00312
00313 for (int i = 0; i < TouchedPrimCount; i++) {
00314
00315 int vertex_index[3];
00316 std::vector<IceMaths::Point> vertex(3);
00317
00318 float x, y, z;
00319 float R;
00320
00321 mesh->getTriangle(TouchedPrim[i], vertex_index[0], vertex_index[1], vertex_index[2]);
00322
00323 for (int j = 0; j < 3; j++) {
00324 mesh->getVertex(vertex_index[j], x, y, z);
00325 TransformPoint4x3(vertex[j], IceMaths::Point(x, y, z), *(mesh->transform));
00326 }
00327
00328 triangle[i] = std::vector<IceMaths::Point> (vertex);
00329
00330 face[i] = IceMaths::Plane(vertex[0], vertex[1], vertex[2]);
00331 face[i].Normalize();
00332
00333 IceMaths::Plane face_s;
00334 IceMaths::TransformPlane(face_s, face[i], sTransInv);
00335 face_s.Normalize();
00336
00337 if (abs(face_s.d) > radius)
00338 cout << "No intersection";
00339 else {
00340
00341 R = sqrt(pow(radius, 2) - pow(face_s.d, 2));
00342 depth[i] = radius - abs(face_s.d);
00343
00344 IceMaths::Point U, V;
00345
00346 TransformPoint3x3(U, vertex[1] - vertex[0], sTransInv);
00347 U.Normalize();
00348 V = face_s.n ^ U;
00349 V.Normalize();
00350
00351 IceMaths::Matrix4x4 scTrans;
00352 scTrans.SetRow(0, U);
00353 scTrans.SetRow(1, V);
00354 scTrans.SetRow(2, face_s.n);
00355 scTrans.SetRow(3, face_s.n * -face_s.d);
00356
00357 IceMaths::Matrix4x4 scTransInv;
00358 IceMaths::InvertPRMatrix(scTransInv, scTrans);
00359
00360 IceMaths::Point vertex_c[3];
00361 std::vector<float> vx, vy;
00362
00363 for (int j = 0; j < 3; j++) {
00364 TransformPoint4x3(vertex_c[j], vertex[j], sTransInv * scTransInv);
00365 vx.push_back(vertex_c[j].x);
00366 vy.push_back(vertex_c[j].y);
00367 }
00368
00369 float cx, cy;
00370 calculateCentroidIntersection(cx, cy, A[i], R, vx, vy);
00371
00372 TransformPoint4x3(q[i], IceMaths::Point (cx, cy, 0), scTrans * sTrans);
00373 }
00374 }
00375
00376 std::vector<bool> considered_checklist(TouchedPrimCount, false);
00377 std::vector<int> sameplane;
00378
00379 std::vector<IceMaths::Point> new_q;
00380 std::vector<IceMaths::Point> new_n;
00381 std::vector<float> new_depth;
00382
00383
00384
00385 for (int i = 0; i < TouchedPrimCount; i++) {
00386
00387 if (!considered_checklist[i]) {
00388
00389 for (int j = i + 1; j < TouchedPrimCount; j++) {
00390 IceMaths::Point normdiff(face[i].n - face[j].n);
00391 if (normdiff.Magnitude() < LOCAL_EPSILON && (face[i].d - face[j].d) < LOCAL_EPSILON) {
00392 if (!sameplane.size()) sameplane.push_back(i);
00393 sameplane.push_back(j);
00394 }
00395 }
00396
00397 if (!sameplane.size()) {
00398 new_q.push_back(q[i]);
00399 new_n.push_back(face[i].n);
00400 new_depth.push_back(depth[i]);
00401 considered_checklist[i] = true;
00402 }
00403 else {
00404
00405 float sum_xA, sum_yA, sum_zA, sum_A;
00406 sum_xA = sum_yA = sum_zA = sum_A = 0;
00407
00408 for (unsigned int k = 0; k < sameplane.size(); k++) {
00409 sum_xA += q[sameplane[k]].x * A[sameplane[k]];
00410 sum_yA += q[sameplane[k]].y * A[sameplane[k]];
00411 sum_zA += q[sameplane[k]].z * A[sameplane[k]];
00412 sum_A += A[sameplane[k]];
00413 considered_checklist[sameplane[k]] = true;
00414 }
00415
00416 IceMaths::Point q_temp;
00417 q_temp.x = sum_xA / sum_A;
00418 q_temp.y = sum_yA / sum_A;
00419 q_temp.z = sum_zA / sum_A;
00420 new_q.push_back(q_temp);
00421 new_n.push_back(face[i].n);
00422 new_depth.push_back(depth[i]);
00423
00424 sameplane.clear();
00425 }
00426 }
00427 }
00428
00429 for (unsigned int i = 0; i < new_q.size(); i++) {
00430 collision_data col;
00431 col.depth = new_depth[i];
00432 col.num_of_i_points = 1;
00433 col.i_point_new[0] = 1;
00434 col.i_point_new[1] = 0;
00435 col.i_point_new[2] = 0;
00436 col.i_point_new[3] = 0;
00437 col.n_vector[0] = sign * new_n[i].x;
00438 col.n_vector[1] = sign * new_n[i].y;
00439 col.n_vector[2] = sign * new_n[i].z;
00440 col.i_points[0][0] = new_q[i].x;
00441 col.i_points[0][1] = new_q[i].y;
00442 col.i_points[0][2] = new_q[i].z;
00443 cdata.push_back(col);
00444 }
00445 }
00446 }
00447 }
00448
00449 else
00450 std::cerr << "SphereCollider::Collide() failed" << std::endl;
00451
00452 }
00453
00454 return result;
00455 }
00456
00457 bool ColdetModelPair::detectPlaneCylinderCollisions(bool detectAllContacts) {
00458
00459 ColdetModelPtr plane, cylinder;
00460 bool reversed=false;
00461 if (models[0]->getPrimitiveType() == ColdetModel::SP_PLANE){
00462 plane = models[0];
00463 }else if(models[0]->getPrimitiveType() == ColdetModel::SP_CYLINDER){
00464 cylinder = models[0];
00465 }
00466 if (models[1]->getPrimitiveType() == ColdetModel::SP_PLANE){
00467 plane = models[1];
00468 reversed = true;
00469 }else if(models[1]->getPrimitiveType() == ColdetModel::SP_CYLINDER){
00470 cylinder = models[1];
00471 }
00472 if (!plane || !cylinder) return false;
00473
00474 IceMaths::Matrix4x4 pTrans = (*(plane->pTransform)) * (*(plane->transform));
00475 IceMaths::Matrix4x4 cTrans = (*(cylinder->pTransform)) * (*(cylinder->transform));
00476
00477 float radius, height;
00478 cylinder->getPrimitiveParam(0, radius);
00479 cylinder->getPrimitiveParam(1, height);
00480
00481 IceMaths::Point pTopLocal(0, height/2, 0), pBottomLocal(0, -height/2, 0);
00482 IceMaths::Point pTop, pBottom;
00483 IceMaths::TransformPoint4x3(pTop, pTopLocal, cTrans);
00484 IceMaths::TransformPoint4x3(pBottom, pBottomLocal, cTrans);
00485
00486 IceMaths::Point pOnPlane, nLocal(0,0,1), n;
00487 IceMaths::TransformPoint3x3(n, nLocal, pTrans);
00488 pTrans.GetTrans(pOnPlane);
00489 float d = pOnPlane|n;
00490
00491 float dTop = (pTop|n) - d;
00492 float dBottom = (pBottom|n) - d;
00493
00494 if (dTop > radius && dBottom > radius) return false;
00495
00496 double theta = asin((dTop - dBottom)/height);
00497 double rcosth = radius*cos(theta);
00498
00499 int contactsCount = 0;
00500 if (rcosth >= dTop) contactsCount+=2;
00501 if (rcosth >= dBottom) contactsCount+=2;
00502
00503 if (contactsCount){
00504 std::vector<collision_data>& cdata = collisionPairInserter->collisions();
00505 cdata.resize(contactsCount);
00506 for (int i=0; i<contactsCount; i++){
00507 cdata[i].num_of_i_points = 1;
00508 cdata[i].i_point_new[0]=1;
00509 cdata[i].i_point_new[1]=0;
00510 cdata[i].i_point_new[2]=0;
00511 cdata[i].i_point_new[3]=0;
00512 if (reversed){
00513 cdata[i].n_vector[0] = -n.x;
00514 cdata[i].n_vector[1] = -n.y;
00515 cdata[i].n_vector[2] = -n.z;
00516 }else{
00517 cdata[i].n_vector[0] = n.x;
00518 cdata[i].n_vector[1] = n.y;
00519 cdata[i].n_vector[2] = n.z;
00520 }
00521 }
00522 IceMaths::Point vBottomTop = pTop - pBottom;
00523 IceMaths::Point v = vBottomTop^n;
00524 v.Normalize();
00525 IceMaths::Point w = v^n;
00526 w.Normalize();
00527
00528 unsigned int index=0;
00529 if (rcosth >= dBottom){
00530 double depth = rcosth - dBottom;
00531 IceMaths::Point iPoint = pBottom - dBottom*n - dBottom*tan(theta)*w;
00532 double x = dBottom/cos(theta);
00533 IceMaths::Point dv = sqrt(radius*radius - x*x)*v;
00534 cdata[index].i_points[0][0] = iPoint.x + dv.x;
00535 cdata[index].i_points[0][1] = iPoint.y + dv.y;
00536 cdata[index].i_points[0][2] = iPoint.z + dv.z;
00537 cdata[index].depth = depth;
00538 index++;
00539 cdata[index].i_points[0][0] = iPoint.x - dv.x;
00540 cdata[index].i_points[0][1] = iPoint.y - dv.y;
00541 cdata[index].i_points[0][2] = iPoint.z - dv.z;
00542 cdata[index].depth = depth;
00543 index++;
00544 }
00545 if (rcosth >= dTop){
00546 double depth = rcosth - dTop;
00547 IceMaths::Point iPoint = pTop - dTop*n - dTop*tan(theta)*w;
00548 double x = dTop/cos(theta);
00549 IceMaths::Point dv = sqrt(radius*radius - x*x)*v;
00550 cdata[index].i_points[0][0] = iPoint.x + dv.x;
00551 cdata[index].i_points[0][1] = iPoint.y + dv.y;
00552 cdata[index].i_points[0][2] = iPoint.z + dv.z;
00553 cdata[index].depth = depth;
00554 index++;
00555 cdata[index].i_points[0][0] = iPoint.x - dv.x;
00556 cdata[index].i_points[0][1] = iPoint.y - dv.y;
00557 cdata[index].i_points[0][2] = iPoint.z - dv.z;
00558 cdata[index].depth = depth;
00559 index++;
00560 }
00561
00562 return true;
00563 }
00564 return false;
00565 }
00566
00567
00568 double ColdetModelPair::computeDistance(double *point0, double *point1)
00569 {
00570 if(models[0]->isValid() && models[1]->isValid()){
00571
00572 Opcode::BVTCache colCache;
00573
00574 colCache.Model0 = &models[1]->dataSet->model;
00575 colCache.Model1 = &models[0]->dataSet->model;
00576
00577 SSVTreeCollider collider;
00578
00579 float d;
00580 Point p0, p1;
00581 collider.Distance(colCache, d, p0, p1,
00582 models[1]->transform, models[0]->transform);
00583 point0[0] = p1.x;
00584 point0[1] = p1.y;
00585 point0[2] = p1.z;
00586 point1[0] = p0.x;
00587 point1[1] = p0.y;
00588 point1[2] = p0.z;
00589 return d;
00590 }
00591
00592 return -1;
00593 }
00594
00595 double ColdetModelPair::computeDistance(int& triangle0, double* point0, int& triangle1, double* point1)
00596 {
00597 if(models[0]->isValid() && models[1]->isValid()){
00598
00599 Opcode::BVTCache colCache;
00600
00601 colCache.Model0 = &models[1]->dataSet->model;
00602 colCache.Model1 = &models[0]->dataSet->model;
00603
00604 SSVTreeCollider collider;
00605
00606 float d;
00607 Point p0, p1;
00608 collider.Distance(colCache, d, p0, p1,
00609 models[1]->transform, models[0]->transform);
00610 point0[0] = p1.x;
00611 point0[1] = p1.y;
00612 point0[2] = p1.z;
00613 point1[0] = p0.x;
00614 point1[1] = p0.y;
00615 point1[2] = p0.z;
00616 triangle1 = colCache.id0;
00617 triangle0 = colCache.id1;
00618 return d;
00619 }
00620
00621 return -1;
00622 }
00623
00624
00625 bool ColdetModelPair::detectIntersection()
00626 {
00627 if(models[0]->isValid() && models[1]->isValid()){
00628
00629 Opcode::BVTCache colCache;
00630
00631 colCache.Model0 = &models[1]->dataSet->model;
00632 colCache.Model1 = &models[0]->dataSet->model;
00633
00634 SSVTreeCollider collider;
00635
00636 return collider.Collide(colCache, tolerance_,
00637 models[1]->transform, models[0]->transform);
00638 }
00639
00640 return false;
00641 }
00642
00643 void ColdetModelPair::setCollisionPairInserter(CollisionPairInserterBase *inserter)
00644 {
00645 delete collisionPairInserter;
00646 collisionPairInserter = inserter;
00647
00648
00649 collisionPairInserter->set(models[1]->dataSet, models[0]->dataSet);
00650 }
00651
00652 int ColdetModelPair::calculateCentroidIntersection(float &cx, float &cy, float &A, float radius, std::vector<float> vx, std::vector<float> vy) {
00653
00654 unsigned int i;
00655 int j[5];
00656 unsigned int k;
00657
00658 int isOk = ColdetModelPair::makeCCW(vx, vy);
00659
00660 if (isOk) {
00661
00662 std::vector<pointStruct> point;
00663 pointStruct p;
00664 unsigned int numInter;
00665 std::vector<float> x_int(2), y_int(2);
00666
00667 for (i = 0; i < vx.size(); i++) {
00668
00669
00670
00671 p.x = vx[i];
00672 p.y = vy[i];
00673 p.angle = atan2(vy[i], vx[i]);
00674 if (p.angle < 0) p.angle += TWOPI;
00675 p.type = vertex;
00676
00677 p.code = isInsideCircle(radius, p.x, p.y);
00678 point.push_back(p);
00679
00680
00681
00682 numInter = calculateIntersection(x_int, y_int, radius, vx[i], vy[i], vx[(i + 1) % vx.size()], vy[(i + 1) % vx.size()]);
00683
00684 for (k = 0; k < numInter; k++) {
00685 p.x = x_int[k];
00686 p.y = y_int[k];
00687 p.angle = atan2(y_int[k], x_int[k]);
00688 if (p.angle < 0) p.angle += TWOPI;
00689 p.type = inter;
00690 p.code = i + 1;
00691 point.push_back(p);
00692 }
00693 }
00694
00695 j[0] = 0;
00696
00697 int start = -1;
00698 bool finished = false;
00699
00700 std::vector<figStruct> figure;
00701 figStruct f;
00702
00703 while (!finished) {
00704
00705 for (int cont = 1; cont <= 4; cont++)
00706 j[cont] = (j[0] + cont) % point.size();
00707
00708 if (point[j[0]].code) {
00709
00710 if (start == -1) start = j[0];
00711
00712 if (point[j[1]].code) {
00713
00714 f.p1 = j[0];
00715 f.p2 = j[1];
00716 f.type = tri;
00717 figure.push_back(f);
00718 j[0] = f.p2;
00719 }
00720
00721 else if (point[j[2]].code || point[j[3]].code || point[j[4]].code) {
00722
00723 f.type = sector;
00724 f.p1 = j[0];
00725
00726 if (point[j[2]].code) f.p2 = j[2];
00727 else if (point[j[3]].code) f.p2 = j[3];
00728 else if (point[j[4]].code) f.p2 = j[4];
00729
00730 figure.push_back(f);
00731 j[0] = f.p2;
00732 }
00733
00734 else {
00735 cout << "Error: No intersection detected" << endl;
00736 return 0;
00737 }
00738 }
00739
00740 else {
00741 j[0] = j[1];
00742 }
00743
00744 if (((j[0] == 0) && (start == -1)) || (j[0] == start))
00745 finished = true;
00746 }
00747
00748 if (figure.size()) {
00749
00750 std::vector<float> x(3, 0);
00751 std::vector<float> y(3, 0);
00752 float sumx, sumy;
00753 float th;
00754
00755 for (k = 0; k < figure.size(); k++) {
00756 if (figure[k].type == tri) {
00757 x[1] = point[figure[k].p1].x;
00758 y[1] = point[figure[k].p1].y;
00759 x[2] = point[figure[k].p2].x;
00760 y[2] = point[figure[k].p2].y;
00761 figure[k].area = calculatePolygonArea(x, y);
00762 sumx = sumy = 0;
00763 for (int cont = 0; cont < 3; cont++) {
00764 sumx += x[cont];
00765 sumy += y[cont];
00766 }
00767 figure[k].cx = sumx / 3;
00768 figure[k].cy = sumy / 3;
00769 }
00770 else if (figure[k].type == sector) {
00771 th = point[figure[k].p2].angle - point[figure[k].p1].angle;
00772 if (th < 0) th += TWOPI;
00773 figure[k].area = pow(radius, 2) * th / 2;
00774 calculateSectorCentroid(figure[k].cx, figure[k].cy, radius, point[figure[k].p1].angle, point[figure[k].p2].angle);
00775 }
00776 }
00777
00778 float sum_xA, sum_yA, sum_A;
00779 sum_xA = sum_yA = sum_A = 0;
00780
00781 for (k = 0; k < figure.size(); k++) {
00782 sum_xA += figure[k].cx * figure[k].area;
00783 sum_yA += figure[k].cy * figure[k].area;
00784 sum_A += figure[k].area;
00785 }
00786
00787 if ((figure.size() == 1) && (sum_A == 0)) {
00788 cx = point[figure[0].p1].x;
00789 cy = point[figure[0].p1].y;
00790 }
00791 else {
00792 cx = sum_xA / sum_A;
00793 cy = sum_yA / sum_A;
00794 }
00795
00796 A = sum_A;
00797
00798 return 1;
00799 }
00800
00801 else {
00802 if (isInsideTriangle(0, 0, vx, vy)) {
00803 cx = cy = 0;
00804 A = TWOPI * pow(radius, 2);
00805 return 1;
00806 }
00807 else{
00808 cx = cy = 0;
00809 A = TWOPI * pow(radius, 2);
00810 return 0;
00811 }
00812 }
00813 }
00814
00815 else
00816 return 0;
00817 }
00818
00819 int ColdetModelPair::makeCCW(std::vector<float> &vx, std::vector<float> &vy) {
00820
00821 if ((vx.size() == 3) && (vy.size() == 3)) {
00822 if (ColdetModelPair::calculatePolygonArea(vx, vy) < 0) {
00823 vx[0] = vx[1];
00824 vy[0] = vy[1];
00825 }
00826 return 1;
00827 }
00828 else {
00829 cout << "The number of vertices does not correspond to a triangle" << endl;
00830 return 0;
00831 }
00832 }
00833
00834 float ColdetModelPair::calculatePolygonArea(const std::vector<float> &vx, const std::vector<float> &vy) {
00835
00836 float area = 0;
00837
00838 if (vx.size() == vy.size()) {
00839 for (unsigned int i = 0; i < vx.size(); i++) {
00840 area += vx[i] * vy[(i + 1) % vx.size()] - vy[i] * vx[(i + 1) % vx.size()];
00841 }
00842 return area / 2;
00843 }
00844 else {
00845 cout << "The number of coordinates does not match" << endl;
00846 return 0;
00847 }
00848 }
00849
00850 #if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
00851 #define trunc(x) ((int)(x))
00852 #endif
00853 void ColdetModelPair::calculateSectorCentroid(float &cx, float &cy, float radius, float th1, float th2) {
00854
00855 float th, psi, phi, g;
00856
00857 th = th2 - th1;
00858 if (th2 < th1) th += TWOPI;
00859
00860 g = (abs(th) > LOCAL_EPSILON) ? 4.0 / 3.0 * radius / th * sin(th / 2) : 2.0 / 3.0 * radius;
00861
00862 psi = th1 + th2;
00863 if (th2 < th1) psi += TWOPI;
00864
00865 phi = psi / 2 - trunc(psi / 2 / TWOPI) * TWOPI;
00866
00867 cx = g * cos(phi);
00868 cy = g * sin(phi);
00869 }
00870
00871 bool ColdetModelPair::isInsideTriangle(float x, float y, const std::vector<float> &vx, const std::vector<float> &vy) {
00872
00873 IceMaths::Point v1, v2;
00874 double m1, m2;
00875 double anglesum = 0;
00876
00877 for (int i = 0; i < 3; i++) {
00878
00879 v1 = IceMaths::Point(vx[i], vy[i], 0) - IceMaths::Point(x, y, 0);
00880 v2 = IceMaths::Point(vx[(i + 1) % vx.size()], vy[(i + 1) % vy.size()], 0) - IceMaths::Point(x, y, 0);
00881
00882 m1 = v1.Magnitude();
00883 m2 = v2.Magnitude();
00884
00885 if (m1 * m2 <= LOCAL_EPSILON) {
00886 anglesum = TWOPI;
00887 break;
00888 }
00889 else
00890 anglesum += acos((v1 | v2) / (m1 * m2));
00891 }
00892
00893 return (abs(TWOPI - anglesum) < LOCAL_EPSILON);
00894 }
00895
00896 int ColdetModelPair::calculateIntersection(std::vector<float> &x, std::vector<float> &y, float radius, float x1, float y1, float x2, float y2) {
00897
00898 int numint;
00899
00900 float x_test, y_test;
00901 x.clear();
00902 y.clear();
00903
00904 float xmin = min(x1, x2);
00905 float xmax = max(x1, x2);
00906 float ymin = min(y1, y2);
00907 float ymax = max(y1, y2);
00908
00909 float v_norm, proy_norm;
00910 float x_temp, y_temp;
00911
00912 std::vector<float> t;
00913
00914 if ((sqrt(pow(x1, 2) + pow(y1, 2)) != radius) && (sqrt(pow(x2, 2) + pow(y2, 2)) != radius)) {
00915
00916 float m, b;
00917 float D;
00918
00919 if (abs(x2 - x1) > LOCAL_EPSILON) {
00920
00921 m = (y2 - y1) / (x2 - x1);
00922 b = y1 - m * x1;
00923
00924 D = 4 * pow(m, 2) * pow(b, 2) - 4 * (1 + pow(m, 2)) * (pow(b, 2) - pow(radius, 2));
00925 }
00926 else
00927 D = pow(radius, 2) - pow(x1, 2);
00928
00929 numint = D < 0 ? 0 : (D > 0 ? 2 : 1);
00930
00931 if (numint > 0) {
00932
00933 for (int i = 0; i < numint; i++) {
00934
00935 if (abs(x2 - x1) > LOCAL_EPSILON) {
00936 x_test = (-2 * m * b + pow(-1.0, i) * sqrt(D)) / (2 * (1 + pow(m, 2)));
00937 y_test = m * x_test + b;
00938 }
00939 else {
00940 x_test = x1;
00941 y_test = pow(-1.0, i) * sqrt(D);
00942 }
00943
00944 cout.flush();
00945
00946 if ((xmin <= x_test) && (x_test <= xmax) && (ymin <= y_test) && (y_test <= ymax)) {
00947 x.push_back(x_test);
00948 y.push_back(y_test);
00949 v_norm = sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
00950 proy_norm = sqrt(pow(x_test - x1, 2) + pow(y_test - y1, 2));
00951 t.push_back(proy_norm / v_norm);
00952 }
00953 }
00954
00955 if (t.size() > 1) {
00956 if (t[0] > t[1]) {
00957 x_temp = x[0];
00958 y_temp = y[0];
00959 x[0] = x[1];
00960 y[0] = y[1];
00961 x[1] = x_temp;
00962 y[1] = y_temp;
00963 }
00964 }
00965 }
00966 }
00967
00968 return t.size();
00969 }
00970
00971 ColdetModelPair& ColdetModelPair::operator=(const ColdetModelPair& org)
00972 {
00973 collisionPairInserter = new CollisionPairInserter;
00974 set(org.models[0], org.models[1]);
00975 tolerance_ = org.tolerance_;
00976 }