ColdetModelPair.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
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     // inverse order because of historical background
00059     // this should be fixed.(note that the direction of normal is inversed when the order inversed 
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         // inverse order because of historical background
00163         // this should be fixed.(note that the direction of normal is inversed when the order inversed 
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);         // Triangle of each face in world's coordinates
00300                                         std::vector<IceMaths::Plane> face(TouchedPrimCount);                            // Plane of each face in world's coordinates 
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;         // Plane of each face in sphere's coordinates
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                                         // The following procedure is needed to merge components from the same plane (but different triangles)
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);  // In order to consider it just once
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; // height and radius of cylinder
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; // center points of top and bottom discs
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; // distance between origin and plane
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){ // bottom disc collides
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){ // top disc collides
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     // inverse order because of historical background
00648     // this should be fixed.(note that the direction of normal is inversed when the order inversed 
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;         // Vertex and Side
00655         int j[5];       // Point ID
00656         unsigned int k;         // Section
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                         // Recording of the vertex
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                         // Recording of the intersections
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 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53