VRUT: /cygdrive/d/src/svn/vrut/trunk/modules/collision_detection/collision

00001 /*
00002  * $Id: Collision_detection.cpp $
00003  *
00004  * Description  : One line description of file.
00005  * Author       : Filip Krystl <mail/Jabber: filip.krystl@gmail.com>  <ICQ: 110199228>
00006  *
00007  * Purpose      :
00008  *     Long description of what the file is for.
00009  */
00010 
00011 
00012 #include "collision_detection.h"
00013 #include "CollisionDetection_BVH.h"
00014 
00015 
00016 #include "../../core/src/geometrynode.h"
00017 #include <stack>
00018 
00019 const int RANDOM_ALG_ITER_COUNT = 50;
00020 
00021 
00022 
00023 
00024 using namespace VRUT;
00025 
00026 
00027 Collision_detection::Collision_detection(const MODULE_ID & _id, const wxString & _name, EventHandler * msgSink)
00028 : SceneModule(_id, _name, 0, msgSink)
00029 {
00031        REGISTER_PARAM_GUI_CHECKBOX(bEnabledParamID, wxT("Enabled"), wxT("false"), wxT("Enable collision detection."));
00032        REGISTER_PARAM_GUI_TEXTCONTROL(idTargerNodeID1ParamID, wxT("Target Node ID 1"), wxT("0"), wxT("Target Node ID 1"));
00033        REGISTER_PARAM_GUI_TEXTCONTROL(idTargerNodeID2ParamID, wxT("Target Node ID 2"), wxT("0"), wxT("Target Node ID 2"));
00034        REGISTER_PARAM_GUI_TEXTCONTROL(fSepareteDistanceParamID, wxT("Minimum separete distance"), wxT("0"), wxT("Minimum separete distance"));
00035        REGISTER_PARAM_GUI_CHECKBOX(bCalculateCollisionPointParamID, wxT("Calculate collision point"), wxT("false"), wxT("Calculate collision point"));
00036        REGISTER_PARAM_GUI_CHECKBOX(bStopTargetNode1ParamID, wxT("Stop target node 1 before collision"), wxT("true"), wxT("Stop target node 1 before collision"));
00037 
00038        REGISTER_PARAM_GUI_CHECKBOX(bWriteComputeTimeToLogParamID, wxT("Computation time to LOG"), wxT("false"), wxT("Write computation time to LOG"));
00039        REGISTER_PARAM_GUI_CHECKBOX(bWriteEventsToLogParamID, wxT("Output events to LOG"), wxT("false"), wxT("Write output events to LOG"));
00040 
00041 
00042        REGISTER_PARAM_GUI_CHECKBOX(bTansformTargetNode1ParamID, wxT("Transform target1"), wxT("true"), wxT("Transform target1"));
00043        REGISTER_PARAM_GUI_TEXTCONTROL(fTansformAxisXParamID, wxT("Transform axis X"), wxT("0"), wxT("Transform axis X"));
00044        REGISTER_PARAM_GUI_TEXTCONTROL(fTansformAxisYParamID, wxT("Transform axis Y"), wxT("0"), wxT("Transform axis Y"));
00045        REGISTER_PARAM_GUI_TEXTCONTROL(fTansformAxisZParamID, wxT("Transform axis Z"), wxT("0"), wxT("Transform axis Z"));
00046 
00047 
00048        bTansformTargetNode1 = true;
00049        fTansformAxisX = 0;
00050        fTansformAxisY = 0;
00051        fTansformAxisZ = 0;
00052 
00053 
00054 
00055        bStopTargetNode1 = true;
00056        bCalculateCollisionPoint = false;
00057        targer1IDs.clear();
00058        targer2IDs.clear();
00059        fSepareteDistance = 0;
00060        bModuleEnabled = false;
00061 
00062        bWriteComputeTimeToLog = false;
00063        bWriteEventsToLog = false;
00064 
00065        bBVHCreated = false;
00066        node1BVH = node2BVH = NULL;
00067 
00068        actualCollisionPlaneNormal = vector3(0,0,1);
00069 
00070 
00071        REGISTER_LISTENER(Event::EVT_SCENE_NODE_TRANSFORMED);
00072 
00073 
00074 }
00075 
00076 
00077 
00078 Collision_detection::~Collision_detection()
00079 {
00080        if (node1BVH)
00081        {
00082               node1BVH->Destroy();
00083               delete node1BVH;
00084        }
00085        if (node2BVH)
00086        {
00087               node2BVH->Destroy();
00088               delete node2BVH;
00089        }
00090 
00091 
00092 
00093 
00094 
00095        for (std::map<NODE_ID,CollisionDetection_BVH*>::iterator it = mapOfBVHs.begin();
00096               it != mapOfBVHs.end(); it++)
00097        {
00098                if ((*it).second)
00099                {
00100                      (*it).second->Destroy();
00101                      delete (*it).second;
00102 
00103                }
00104        }
00105 
00106 
00107 }
00108 
00109 
00110 
00111 void Collision_detection::processEvent(wxCommandEvent & evt)
00112 {
00114        SceneModule::processEvent(evt);
00115               
00116 
00117        wxString _strTarget1;
00118        wxString _strTarget2;
00119 
00120 
00121        switch (evt.GetEventType())
00122        {
00124        case Event::EVT_PARAM_SET:
00125               UPDATE_PARAM_FROM_EVENT_BOOL(bEnabledParamID, bModuleEnabled, evt);
00126               UPDATE_PARAM_FROM_EVENT_STRING(idTargerNodeID1ParamID, _strTarget1, evt);
00127               UPDATE_PARAM_FROM_EVENT_STRING(idTargerNodeID2ParamID, _strTarget2, evt);
00128 
00129               UPDATE_PARAM_FROM_EVENT_INT(fSepareteDistanceParamID, fSepareteDistance, evt);
00130               UPDATE_PARAM_FROM_EVENT_BOOL(bCalculateCollisionPointParamID, bCalculateCollisionPoint, evt);
00131               UPDATE_PARAM_FROM_EVENT_BOOL(bStopTargetNode1ParamID, bStopTargetNode1, evt);
00132 
00133               UPDATE_PARAM_FROM_EVENT_BOOL(bWriteEventsToLogParamID, bWriteEventsToLog, evt);
00134               UPDATE_PARAM_FROM_EVENT_BOOL(bWriteComputeTimeToLogParamID, bWriteComputeTimeToLog, evt);
00135 
00136 
00137               UPDATE_PARAM_FROM_EVENT_BOOL(bTansformTargetNode1ParamID, bTansformTargetNode1, evt);
00138               UPDATE_PARAM_FROM_EVENT_FLOAT(fTansformAxisXParamID, fTansformAxisX, evt);
00139               UPDATE_PARAM_FROM_EVENT_FLOAT(fTansformAxisYParamID, fTansformAxisY, evt);
00140               UPDATE_PARAM_FROM_EVENT_FLOAT(fTansformAxisZParamID, fTansformAxisZ, evt);
00141 
00142 
00143 
00144               if ((_strTarget1 != strTarget1 && !_strTarget1.IsEmpty()) || (_strTarget2 != strTarget2 && !_strTarget2.IsEmpty()))
00145               {
00146                      bBVHCreated = false;
00147 
00148                      if (!_strTarget1.IsEmpty())
00149                             strTarget1 = _strTarget1;
00150 
00151                      if (!_strTarget2.IsEmpty())
00152                             strTarget2 = _strTarget2;
00153 
00154                      targer1IDs.clear();
00155                      targer2IDs.clear();
00156                      targer1TabuIDs.clear();
00157                      targer2TabuIDs.clear();
00158 
00159                      MATRIX m;
00160                      oldTarget1WorldMATRIX = target1WorldMATRIX = m;
00161 
00162 
00163                      parseTargetStrings(strTarget1,&targer1IDs,&targer1TabuIDs);
00164                      parseTargetStrings(strTarget2,&targer2IDs,&targer2TabuIDs);
00165               }
00166 
00167               
00168               break;
00169 
00170        case Event::EVT_SCENE_NODE_TRANSFORMED:
00171 
00172 
00173               clock_t t1=clock();
00174               
00175 
00176               if (node1BVH && node2BVH)
00177               {
00178                      std::deque<NODE_ID>* nodeList = new std::deque<NODE_ID>();
00179 
00180                      getGeometryNodesFromSceneNode(sceneMgr->GetScene(sceneID)->GetNode((NODE_ID)evt.GetInt()),nodeList);
00181 
00182 
00183                      for (std::deque<NODE_ID>::iterator it = nodeList->begin();
00184                             it != nodeList->end(); it++)
00185                      {
00186                             node1BVH->SetInvalid(node1BVH->GetBVHNodeAssignedTo(*it));
00187                             node2BVH->SetInvalid(node2BVH->GetBVHNodeAssignedTo(*it));
00188                      }
00189 
00190                      delete nodeList;
00191 
00192                      node1BVH->UpdateBV();
00193                      node2BVH->UpdateBV();
00194               }
00195 
00196               for (std::map<NODE_ID,CollisionDetection_BVH*>::iterator it = mapOfBVHs.begin(); it != mapOfBVHs.end(); it++)
00197               {
00198                       if ((*it).second)
00199                       {
00200                              NODE_ID nodeId =(*it).second->TargetGeometryNode;
00201                              const MATRIX* m = sceneMgr->GetScene(sceneID)->GetNode(nodeId)->GetWorldTransMatrix();
00202 
00203 
00204                              bool bRet = true;
00205                              for (int i=0; i<16; i++)
00206                              {
00207                                     if (m->_m[i] != (*it).second->CurrMatrix._m[i])
00208                                           bRet = false;
00209                              }
00210 
00211                              if (!bRet)
00212                              {
00213                                     (*it).second->SetCurrentMatrix(*m);
00214                                     (*it).second->UpdateBVH(NULL);
00215                              }
00216 
00217                       }
00218               }
00219 
00220               if (targer1IDs.size() > 0)
00221               {
00222                      const MATRIX* m = sceneMgr->GetScene(sceneID)->GetNode(targer1IDs[0])->GetWorldTransMatrix();
00223 
00224                       
00225                      bool bRet = true;
00226                      for (int i=0; i<16; i++)
00227                      {
00228                             if (m->_m[i] != target1WorldMATRIX._m[i])
00229                             {
00230                                    bRet = false;
00231                                    break;
00232                             }
00233                      }
00234 
00235                      if (!bRet)
00236                      {
00237 
00238                             oldTarget1WorldMATRIX = MATRIX(target1WorldMATRIX);
00239                             target1WorldMATRIX = MATRIX(*sceneMgr->GetScene(sceneID)->GetNode(targer1IDs[0])->GetWorldTransMatrix());
00240 
00241                             MATRIX m;
00242                             if (oldTarget1WorldMATRIX == m)
00243                                    oldTarget1WorldMATRIX = target1WorldMATRIX;
00244                      }
00245               }
00246 
00247 
00248               if (bWriteComputeTimeToLog)
00249               {
00250                      clock_t t2=clock();
00251                      wxString str;
00252                      str.append(wxT("<Collision_detection> BVHs updated after nodes tranformed in "));
00253                      double d = double(t2-t1);
00254 
00255                      str << d;
00256 
00257 
00258                      LOG(str);
00259               }
00260 
00261 
00262               break;
00263 
00264        }
00265 }
00266 
00267 void Collision_detection::parseTargetStrings(wxString strToParse, std::vector<NODE_ID>* IDs, std::vector<NODE_ID>* tabuIDs )
00268 {
00269        // Parse string for target1
00270        size_t pos = 0;
00271        double d = 0;
00272        wxString s = strToParse;
00273 
00274 
00275        // Check if there are more numbers after ";"
00276        while (true)
00277        {
00278               pos = s.find(_T(';'));
00279 
00280               if (pos == wxString::npos)
00281               {
00282                      
00283                      if (s.ToDouble(&d))
00284                      {
00285                             if (d < 0)
00286                             {
00287                                    tabuIDs->push_back(abs(d));
00288                             }
00289                             else
00290                             {
00291                                    IDs->push_back(d);
00292                             }
00293                      }
00294                      break;
00295               }
00296 
00297 
00298               wxString st = s.Left(pos);
00299               s.Remove(0,pos+1);
00300 
00301               if (st.ToDouble(&d))
00302               {
00303                      if (d < 0)
00304                      {
00305                             tabuIDs->push_back(abs(d));
00306                      }
00307                      else
00308                      {
00309                             IDs->push_back(d);
00310                      }
00311               }
00312 
00313 
00314        }
00315 }
00316 
00317 
00318 void Collision_detection::getGeometryNodesFromBVHNode(const BVHNode* pNode, std::deque<NODE_ID>* nodeList)
00319 {
00320        if (pNode)
00321        {
00322               if (pNode->GetSceneNodeID() != NODE_ID_NONE)
00323               {
00324                      nodeList->push_back(pNode->GetSceneNodeID());
00325               }
00326               else
00327               {
00328                      getGeometryNodesFromBVHNode(pNode->GetLChild(),nodeList);
00329                      getGeometryNodesFromBVHNode(pNode->GetRChild(),nodeList);
00330               }
00331        }
00332 }
00333 
00334 
00335 void Collision_detection::getGeometryNodesFromSceneNode(const SceneNode* pNode, std::deque<NODE_ID>* nodeList)
00336 {
00337        if (pNode && pNode->IsActive())
00338        {
00339               if (pNode->IsOfType(SceneNode::GEOMETRY))
00340               {
00341                      nodeList->push_back(pNode->GetID());
00342               }
00343               else
00344               {
00345                      for (std::list<NODE_ID>::const_iterator it = pNode->GetChildren()->begin();
00346                             it != pNode->GetChildren()->end();it++)
00347                      {
00348                             getGeometryNodesFromSceneNode(GetSceneMgr()->GetScene(sceneID)->GetNode(*it),nodeList);
00349                      }
00350               }
00351        }
00352 }
00353 
00354 void Collision_detection::createBVHs()
00355 {
00356        Scene* scene = GetSceneMgr()->GetScene(sceneID);
00357        // BVH of second target
00358        if (node2BVH)
00359        {
00360               node2BVH->Destroy();
00361               delete node2BVH;
00362        }
00363 
00364        node2BVH = new BVH(GetSceneMgr()->GetScene(sceneID));
00365        std::deque<NODE_ID>* nodeList1 = new std::deque<NODE_ID>();
00366        std::deque<NODE_ID>* nodeList2 = new std::deque<NODE_ID>();
00367 
00368        if (targer2IDs.size() > 0)
00369        {
00370               for (int i = 0; i < (int)targer2IDs.size(); i++)
00371               {
00372                      getGeometryNodesFromSceneNode(scene->GetNode(targer2IDs[i]),nodeList2);
00373               }
00374        }
00375        else
00376        {
00377               getGeometryNodesFromSceneNode(scene->GetNode(scene->GetRootID()),nodeList2);
00378        }
00379 
00380 
00381 
00382        // BVH of first target
00383 
00384        if (node1BVH)
00385        {
00386               node1BVH->Destroy();
00387               delete node1BVH;
00388        }
00389 
00390        node1BVH = new BVH(GetSceneMgr()->GetScene(sceneID));
00391 
00392        for (int i = 0; i < (int)targer1IDs.size(); i++)
00393        {
00394               getGeometryNodesFromSceneNode(scene->GetNode(targer1IDs[i]),nodeList1);
00395        }
00396 
00397 
00398 
00399 
00400        for (std::deque<NODE_ID>::iterator it1 = nodeList1->begin();
00401               it1 != nodeList1->end(); it1++)
00402        {
00403               for (std::deque<NODE_ID>::iterator it2 = nodeList2->begin();
00404                      it2 != nodeList2->end(); it2++)
00405               {
00406                      if ((*it1) == (*it2))
00407                      {
00408                             nodeList2->erase(it2);
00409                             break;
00410                      }
00411               }
00412 
00413        }
00414 
00415        for (int i = 0; i < (int)targer2TabuIDs.size(); i++)
00416        {
00417               for (std::deque<NODE_ID>::iterator it2 = nodeList2->begin();
00418                             it2 != nodeList2->end(); it2++)
00419               {
00420                      if ((*it2) == targer2TabuIDs[i])
00421                      {
00422                             nodeList2->erase(it2);
00423                             break;
00424                      }
00425               }
00426        }
00427 
00428 
00429 
00430        node2BVH->Build(nodeList2);
00431        node2BVH->UpdateBV();
00432 
00433 
00434        node1BVH->Build(nodeList1);
00435        node1BVH->UpdateBV();
00436 
00437 
00438        clock_t t1=clock();
00439 
00440 
00441        for (std::deque<NODE_ID>::iterator it3 = nodeList1->begin();
00442               it3 != nodeList1->end(); it3++)
00443        {
00444               const SceneNode* node = scene->GetNode(*it3);
00445               if (node->GetType() == GeometryNode::GEOMETRY)
00446               {
00447                      const GeometryNode* gNode = (GeometryNode*)node;
00448                      const Geometry* g = scene->GetGeometry(gNode->GetGeometryID());
00449 
00450 
00451                      std::vector<Triangle> triangleList1 = g->Triangulate();
00452 
00453                      CollisionDetection_BVH* bvh = new CollisionDetection_BVH((*it3),*scene->GetNode(*it3)->GetWorldTransMatrix());
00454                      bvh->Build(&triangleList1);
00455                      bvh->UpdateBVH(NULL);
00456                      mapOfBVHs.insert(bvh_Pair((*it3),bvh));
00457               }
00458        }
00459 
00460        if (bWriteComputeTimeToLog)
00461        {
00462               clock_t t2=clock();
00463 
00464 
00465               double t = t2 - t1;
00466 
00467               wxString str;
00468               str.append(wxT("<collision detection> 1 cycle has been made in "));
00469               double d = double(t2-t1);
00470 
00471               str << d;
00472 
00473               LOG(str);
00474 
00475        }
00476 
00477        delete nodeList1;
00478        delete nodeList2;
00479 
00480        if (node1BVH->GetRoot() != NULL && node2BVH->GetRoot() != NULL)
00481               bBVHCreated = true;
00482 
00483 }
00484 
00485 void Collision_detection::run()
00486 {
00487 
00488        if (!bModuleEnabled)
00489               return;
00490 
00491        Scene* scene = GetSceneMgr()->GetScene(sceneID);
00492        if (scene)
00493        {
00494               clock_t t1=clock();
00495 
00496               if (!bBVHCreated && targer1IDs.size() > 0)
00497               {
00498                      createBVHs();
00499               }
00500 
00501 
00502 
00503               if (bBVHCreated)
00504               {
00505                      if (!findIntersection(node1BVH->GetRoot(), node2BVH->GetRoot()))
00506                      {
00507                             //MATRIX m = MATRIX::Translation(vector3(0.1,0,0));
00508                             //sceneMgr->GetScene(sceneID)->Transform(7,m);
00509                      }
00510               }
00511 
00512               if ( bTansformTargetNode1)
00513               {
00514 
00515 
00516                      if (fTansformAxisX != 0 || fTansformAxisY != 0 || fTansformAxisZ != 0)
00517                      {
00518                             MATRIX m = MATRIX::Translation(vector3(fTansformAxisX,fTansformAxisY,fTansformAxisZ));
00519                             for (int i = 0; i < (int)targer1IDs.size(); i++)
00520                             {
00521                                    sceneMgr->GetScene(sceneID)->Transform(targer1IDs[i],m);
00522                             }
00523                      }
00524               }
00525 
00526               if (bWriteComputeTimeToLog)
00527               {
00528                      clock_t t2=clock();
00529                      wxString str;
00530                      str.append(wxT("<Collision_detection> 1 cycle has been made in "));
00531                      double d = double(t2-t1);
00532 
00533                      str << d;
00534 
00535                      LOG(str);
00536               }
00537 
00538 
00539        }
00540 }
00541 
00542 
00543 
00544 //void Collision_detection::findIntersection9(BvhNode * pNode1, BvhNode * pNode2,std::vector<SAH_CollisionPair>* CollisionNodes)
00545 //{
00546 //     std::stack<SAH_CollisionPair> list;
00547 //
00548 //     SAH_CollisionPair pair;
00549 //     pair.pNodeL = pNode1;
00550 //     pair.pNodeR = pNode2;
00551 //
00552 //
00553 //     list.push(pair);
00554 //
00555 //     Collison_Detector_IFace* detector = CollisionDetector::GetColisionDetector(CollisionDetector::DT_AABB);
00556 //
00557 //     while(!list.empty())
00558 //     {
00559 //            SAH_CollisionPair pair = list.top();
00560 //            list.pop();
00561 //
00562 //            CollisionReport collisionReport = detector->FindCollision((void*)&pair.pNodeL->box, (void*)&pair.pNodeR->box);
00563 //            if (collisionReport.bCollide)
00564 //            {
00565 //                   if (pair.pNodeL->IsLeaf() && pair.pNodeR->IsLeaf())
00566 //                   {
00567 //                          CollisionNodes->push_back(pair);
00568 //                   }
00569 //                   else
00570 //                   {
00571 //                          // Descend rule
00572 //                          if ((pair.pNodeR->box.Volume() > pair.pNodeL->box.Volume()) && !pair.pNodeR->IsLeaf())//pNodeR->GetSceneNodeID() == NODE_ID_NONE)
00573 //                          {
00574 //                                 BvhInterior* pNodeRI = (BvhInterior*)pair.pNodeR;
00575 //
00576 //                                 list.push(SAH_CollisionPair(pair.pNodeL,pNodeRI->front));
00577 //                                 list.push(SAH_CollisionPair(pair.pNodeL,pNodeRI->back));
00578 //                          }
00579 //                          else if (!pair.pNodeL->IsLeaf())
00580 //                          {
00581 //                                 BvhInterior* pNodeLI = (BvhInterior*)pair.pNodeL;
00582 //                                 list.push(SAH_CollisionPair(pNodeLI->front,pair.pNodeR));
00583 //                                 list.push(SAH_CollisionPair(pNodeLI->back,pair.pNodeR));
00584 //
00585 //                          }
00586 //                          //else
00587 //                          //{
00588 //                          //     LOG(wxT("<Collision_detection> A problem emerged while detecting"));
00589 //                          //}
00590 //                   }
00591 //
00592 //
00593 //            }
00594 //
00595 //
00596 //
00597 //     }
00598 //
00599 //
00600 //}
00601 
00602 
00603 
00604 void Collision_detection::findCollisionPairsInGeometries(CollisionDetection_BVHNode * pNode1, CollisionDetection_BVHNode * pNode2,std::vector<CollisionDetection_CollisionPair>* CollisionNodes)
00605 {
00606        std::stack<CollisionDetection_CollisionPair> list;
00607 
00608        CollisionDetection_CollisionPair pair;
00609        pair.pNodeL = pNode1;
00610        pair.pNodeR = pNode2;
00611 
00612 
00613        list.push(pair);
00614 
00615        Collison_Detector_IFace* detector = CollisionDetector::GetColisionDetector(CollisionDetector::DT_AABB);
00616 
00617        while(!list.empty() && detector)
00618        {
00619               CollisionDetection_CollisionPair pair = list.top();
00620               list.pop();
00621 
00622               if (!pair.pNodeL || !pair.pNodeR)
00623                      continue;
00624 
00625               AABB box1(*pair.pNodeL->GetAABB());
00626               AABB box2(*pair.pNodeR->GetAABB());
00627 
00628               if (fSepareteDistance > 0)
00629               {
00630                      box1.MaxBound += VECTOR3(fSepareteDistance/2,fSepareteDistance/2,fSepareteDistance/2);
00631                      box1.MinBound -= VECTOR3(fSepareteDistance/2,fSepareteDistance/2,fSepareteDistance/2);
00632 
00633                      box2.MaxBound += VECTOR3(fSepareteDistance/2,fSepareteDistance/2,fSepareteDistance/2);
00634                      box2.MinBound -= VECTOR3(fSepareteDistance/2,fSepareteDistance/2,fSepareteDistance/2);
00635               }
00636 
00637               CollisionReport collisionReport = detector->FindCollision((void*)&box1, (void*)&box2);
00638               if (collisionReport.bCollide)
00639               {
00640                      if (!pair.pNodeL->GetTargetTriangles()->empty() && !pair.pNodeR->GetTargetTriangles()->empty())
00641                      {
00642                             CollisionNodes->push_back(pair);
00643                      }
00644                      else
00645                      {
00646                             // Descend rule
00647                             if (((pair.pNodeR->GetBSphere()->Radius > pair.pNodeL->GetBSphere()->Radius) && pair.pNodeR->GetTargetTriangles()->empty())
00648                                    || !pair.pNodeL->GetTargetTriangles()->empty())
00649                             {
00650                                    list.push(CollisionDetection_CollisionPair(pair.pNodeL,pair.pNodeR->GetLChild()));
00651                                    list.push(CollisionDetection_CollisionPair(pair.pNodeL,pair.pNodeR->GetRChild()));
00652                             }
00653                             else if (pair.pNodeL->GetTargetTriangles()->empty())
00654                             {
00655                                    list.push(CollisionDetection_CollisionPair(pair.pNodeL->GetLChild(),pair.pNodeR));
00656                                    list.push(CollisionDetection_CollisionPair(pair.pNodeL->GetRChild(),pair.pNodeR));
00657 
00658                             }
00659                             //else
00660                             //{
00661                             //     LOG(wxT("<Collision_detection> A problem emerged while detecting"));
00662                             //}
00663                      }
00664 
00665 
00666               }
00667 
00668 
00669 
00670        }
00671 
00672        delete detector;
00673 
00674 }
00675 
00676 
00677 
00678 bool Collision_detection::findIntersection(const BVHNode * pNode1, const BVHNode * pNode2)
00679 {
00680        bool bRet = false;
00681 
00682        std::stack<CollisionPair> list;
00683 
00684        CollisionPair pair;
00685        pair.pNodeL = pNode1;
00686        pair.pNodeR = pNode2;
00687 
00688 
00689        list.push(pair);
00690 
00691        Collison_Detector_IFace* detector = CollisionDetector::GetColisionDetector(CollisionDetector::DT_AABB);
00692 
00693        while(!list.empty() && detector)
00694        {
00695               CollisionPair pair = list.top();
00696               list.pop();
00697 
00698               if (!pair.pNodeL || !pair.pNodeR)
00699                      continue;
00700 
00701 
00702 
00703 
00704               AABB box1(*pair.pNodeL->GetAABB());
00705               AABB box2(*pair.pNodeR->GetAABB());
00706 
00707               if (fSepareteDistance > 0)
00708               {
00709                      box1.MaxBound += VECTOR3(fSepareteDistance/2,fSepareteDistance/2,fSepareteDistance/2);
00710                      box1.MinBound -= VECTOR3(fSepareteDistance/2,fSepareteDistance/2,fSepareteDistance/2);
00711 
00712                      box2.MaxBound += VECTOR3(fSepareteDistance/2,fSepareteDistance/2,fSepareteDistance/2);
00713                      box2.MinBound -= VECTOR3(fSepareteDistance/2,fSepareteDistance/2,fSepareteDistance/2);
00714               }
00715 
00716               CollisionReport collisionReport = detector->FindCollision((void*)&box1, (void*)&box2);
00717               if (collisionReport.bCollide)
00718               {
00719                      if (pair.pNodeL->GetSceneNodeID() != NODE_ID_NONE && pair.pNodeR->GetSceneNodeID() != NODE_ID_NONE)
00720                      {
00721                             CollisionReport * collisionReport = new CollisionReport();
00722 
00723                             testPrimitives(pair.pNodeL, pair.pNodeR, collisionReport);
00724                             if (collisionReport->bCollide)
00725                             {
00726                                    collisionReport->collisionNode1 = pair.pNodeL->GetSceneNodeID();
00727                                    collisionReport->collisionNode2 = pair.pNodeR->GetSceneNodeID();
00728 
00729 
00730                                    wxCommandEvent evt = Event::GET_EVT_SCENE_COLLISION(sceneID, collisionReport);
00731                                    PostEvent(evt);
00732                                    
00733 
00734 
00735                                    bRet = true;
00736                                    if (bWriteEventsToLog)
00737                                    {
00738                                           reportCollision(pair.pNodeL, pair.pNodeR);
00739                                    }
00740 
00741                                    MATRIX m2 = MATRIX::Translation(vector3(-0.1,0,0));
00742 
00743                                    if (bStopTargetNode1)
00744                                    {
00745                                           MATRIX x = target1WorldMATRIX;
00746                                           x.InvertThisPrecise();
00747                                           MATRIX mm = x * oldTarget1WorldMATRIX;
00748 
00749 
00750                                           MATRIX mmX =  x * target1WorldMATRIX;
00751 
00752                                           MATRIX mm2 = MATRIX::Translation(mm.ExtractTranslation());
00753 
00754                                           sceneMgr->GetScene(sceneID)->Transform(targer1IDs[0], mm2 );
00755 
00756                                           //std::deque<NODE_ID>* nodeList = new std::deque<NODE_ID>();
00757 
00758                                           //for (int i = 0; i < (int)targer1IDs.size(); i++)
00759                                           //{
00760                                           //     getGeometryNodesFromSceneNode(sceneMgr->GetScene(sceneID)->GetNode(targer1IDs[i]),nodeList);
00761                                           //}
00762                                           //
00763                                           //for (std::deque<NODE_ID>::iterator it = nodeList->begin();
00764                                           //     it != nodeList->end(); it++)
00765                                           //{
00766                                           //     std::map<NODE_ID,CollisionDetection_BVH*>::iterator it1 = mapOfBVHs.find((*it));
00767                                           //     if (it1 != mapOfBVHs.end())
00768                                           //     {
00769                                           //            MATRIX old = (*it1).second->OldMatrix;
00770                                           //            MATRIX now = MATRIX((*it1).second->CurrMatrix);
00771                                           //            MATRIX mm = now.Inverse() * old;
00772                                           //            MATRIX mm2 = MATRIX::Translation(mm.ExtractTranslation());
00773 
00774                                           //            sceneMgr->GetScene(sceneID)->Transform((*it1).first, mm2 );
00775                                           //     }
00776                                           //}
00777                                    }
00778 
00779 
00780 
00781                                    if (collisionReport->collisionPoints.size() > 0)
00782                                    {
00783                                           if (bWriteEventsToLog)
00784                                           {
00785                                                  wxString s = wxT("<Collision_detection> Collision point [x,y,z]=");
00786                                                  s << collisionReport->collisionPoints[0].ToString();
00787 
00788 
00789                                                  LOG(s);
00790                                           }
00791 
00792                                           //const MATRIX* wm = sceneMgr->GetScene(sceneID)->GetNode(13)->GetWorldTransMatrix();
00793 
00794                                           //vector3 tr = wm->ExtractTranslation();
00795                                           //MATRIX m2 = MATRIX::Translation(collisionReport->collisionPoints[0] - tr);
00796 
00797                                           //sceneMgr->GetScene(sceneID)->Transform(13,m2);
00798 
00799 
00800                                    }
00801 
00802                                    if (collisionReport->collisionPlaneNormals.size() > 0)
00803                                    {
00804                                           if (bWriteEventsToLog)
00805                                           {
00806                                                  wxString s = wxT("<Collision_detection> Collision plane normal [x,y,z]=");
00807                                                  s << collisionReport->collisionPlaneNormals[0].ToString();
00808                                                  LOG(s);
00809                                           }
00810 
00812                                           //const MATRIX* wm = sceneMgr->GetScene(sceneID)->GetNode(10)->GetWorldTransMatrix();
00813 
00814                                           //vector3 tr = wm->ExtractTranslation();
00815                                           //MATRIX m2 = MATRIX::Translation(collisionReport->collisionPoints[0] - tr);
00816 
00817                                           //float dot = actualCollisionPlaneNormal.Dot(collisionReport->collisionPlaneNormals[0]);
00818 
00819                                           //float f = abs(abs(dot) - 1);
00820 
00821                                           //if (f > 0.01)
00822                                           //{
00823                                           //     MATRIX m3 = MATRIX::RotationVectors(collisionReport->collisionPlaneNormals[0],actualCollisionPlaneNormal);
00824                                           //     //m3 = m3.Inverse();
00825                                           //     actualCollisionPlaneNormal = collisionReport->collisionPlaneNormals[0];
00826                                           //     //sceneMgr->GetScene(sceneID)->Transform(10,m2);
00827                                           //     sceneMgr->GetScene(sceneID)->Transform(10,m3);
00828 
00829                                           //}
00830                                    }
00831                             }
00832                             else
00833                             {
00834                                    delete collisionReport;
00835                             }
00836 
00837                      }
00838                      else
00839                      {
00840                             // Descend rule
00841                             if (((pair.pNodeR->GetBSphere()->Radius > pair.pNodeL->GetBSphere()->Radius) && pair.pNodeR->GetSceneNodeID() == NODE_ID_NONE)
00842                                    || pair.pNodeL->GetSceneNodeID() != NODE_ID_NONE)
00843                             {
00844                                    list.push(CollisionPair(pair.pNodeL,pair.pNodeR->GetLChild()));
00845                                    list.push(CollisionPair(pair.pNodeL,pair.pNodeR->GetRChild()));
00846                             }
00847                             else if (pair.pNodeL->GetSceneNodeID() == NODE_ID_NONE)
00848                             {
00849                                    list.push(CollisionPair(pair.pNodeL->GetLChild(),pair.pNodeR));
00850                                    list.push(CollisionPair(pair.pNodeL->GetRChild(),pair.pNodeR));
00851 
00852                             }
00853                      }
00854 
00855 
00856               }
00857 
00858 
00859 
00860        }
00861 
00862        delete detector;
00863        return bRet;
00864 }
00865 
00866 
00867 
00868 void Collision_detection::testPrimitives(const BVHNode * pNode1, const BVHNode * pNode2, CollisionReport* collisionReport)
00869 {
00870        if (collisionReport == NULL)
00871               return;
00872 
00873 
00874        Scene* pScene = GetSceneMgr()->GetScene(sceneID);
00875 
00876 
00877        std::vector<CollisionArea> CollisionAreaList;
00878 
00879 
00880        collisionReport->bCollide = false;
00881 
00882        clock_t t1=clock();
00883 
00884        if (pScene)
00885        {
00886               GeometryNode* gNode1 = (GeometryNode*)pScene->GetNode(pNode1->GetSceneNodeID());
00887               const Geometry* pGeometry1 = pScene->GetGeometry(gNode1->GetGeometryID());
00888 
00889               GeometryNode* gNode2 = (GeometryNode*)pScene->GetNode(pNode2->GetSceneNodeID());
00890               const Geometry* pGeometry2 = pScene->GetGeometry(gNode2->GetGeometryID());
00891 
00892               if (pGeometry1  != NULL
00893                      && pGeometry2 != NULL)
00894               {
00895 
00896                      std::map<NODE_ID,CollisionDetection_BVH*>::iterator it1 = mapOfBVHs.find(pNode1->GetSceneNodeID());
00897                      if (it1 == mapOfBVHs.end())
00898                      {
00899                             std::vector<Triangle> triangleList1 = pGeometry1->Triangulate();
00900 
00901                             CollisionDetection_BVH* bvh = new CollisionDetection_BVH(pNode1->GetSceneNodeID(),*gNode1->GetWorldTransMatrix());
00902                             //t1=clock();
00903 
00904                             bvh->Build(&triangleList1);
00905                             bvh->UpdateBVH(NULL);
00906 
00907                             if (bWriteComputeTimeToLog)
00908                             {
00909                                    clock_t t2=clock();
00910                                    wxString str;
00911                                    str.append(wxT("<Collision_detection> Built BVH for node "));
00912                                    str << pNode1->GetSceneNodeID();
00913                                    str.append(wxT(" with "));
00914                                    str << (unsigned int)triangleList1.size();
00915                                    str.append(wxT(" triangles, time cost "));
00916                                    double d = double(t2-t1);
00917                                    str << d;
00918                                    LOG(str);
00919                             }
00920 
00921                             mapOfBVHs.insert(bvh_Pair(pNode1->GetSceneNodeID(),bvh));
00922                             it1 = mapOfBVHs.find(pNode1->GetSceneNodeID());
00923                      }
00924 
00925 
00926                      std::map<NODE_ID,CollisionDetection_BVH*>::iterator it2 = mapOfBVHs.find(pNode2->GetSceneNodeID());
00927                      if (it2 == mapOfBVHs.end())
00928                      {
00929                             std::vector<Triangle> triangleList2 = pGeometry2->Triangulate();
00930 
00931                             clock_t t7 = clock();
00932                             CollisionDetection_BVH* bvh2 = new CollisionDetection_BVH(pNode2->GetSceneNodeID(),*gNode2->GetWorldTransMatrix());
00933                             bvh2->Build(&triangleList2);
00934                             bvh2->UpdateBVH(NULL);
00935 
00936                             if ( bWriteComputeTimeToLog)
00937                             {
00938                                    clock_t t8 = clock();
00939 
00940                                    wxString str;
00941                                    str.append(wxT("<Collision_detection> Built BVH for node "));
00942                                    str << pNode2->GetSceneNodeID();
00943                                    str.append(wxT(" with "));
00944                                    str << (unsigned int)triangleList2.size();
00945 
00946                                    str.append(wxT(" triangles, time cost "));
00947 
00948                                    double d = double(t8-t7);
00949 
00950                                    str << d;
00951                                    LOG(str);
00952                             }
00953 
00954                             mapOfBVHs.insert(bvh_Pair(pNode2->GetSceneNodeID(),bvh2));
00955 
00956                             it2 = mapOfBVHs.find(pNode2->GetSceneNodeID());
00957 
00958                      }
00959 
00960 
00961 
00962                      //t1=clock();
00963 
00964                      std::vector<CollisionDetection_CollisionPair>* CollisionNodes2 = new std::vector<CollisionDetection_CollisionPair>();
00965 
00966                      findCollisionPairsInGeometries((*it1).second->root,(*it2).second->root,CollisionNodes2);
00967 
00968 
00969 
00970                      Collison_Detector_IFace* pDetector = NULL; //CollisionDetector::GetColisionDetector(CollisionDetector::DT_Triangle);
00971 
00972                      if (fSepareteDistance > 0)
00973                             pDetector = CollisionDetector::GetColisionDetector(CollisionDetector::DT_Triangle_Distance);
00974                      else
00975                             pDetector = CollisionDetector::GetColisionDetector(CollisionDetector::DT_Triangle);
00976 
00977 
00978                      //wxString str1;
00979                      //str1.append(wxT("<Collision_detection> After traversing BVH "));
00980                      //str1 << (int)CollisionNodes2->size();
00981                      //str1.append(wxT(" collision pairs has been found with "));
00982                      //str1 << (int)TRIANGLES_IN_LEAF;
00983                      //str1.append(wxT(" trinagles in leafs"));
00984 
00985                      //LOG(str1);
00986 
00987 
00988 
00989 
00990 
00991                      for (std::vector<CollisionDetection_CollisionPair>::iterator it = CollisionNodes2->begin();
00992                             it != CollisionNodes2->end(); it++)
00993                      {
00994                             for (std::vector<Triangle>::iterator it2 = (*it).pNodeL->GetTargetTriangles()->begin();
00995                                    it2 != (*it).pNodeL->GetTargetTriangles()->end();it2++)
00996                             {
00997                                    for (std::vector<Triangle>::iterator it3 = (*it).pNodeR->GetTargetTriangles()->begin();
00998                                           it3 != (*it).pNodeR->GetTargetTriangles()->end();it3++)
00999                                    {
01000                                           //Triangle triangle1 = triangleList1[iNodeId + i];
01001                                           //triangle1.v1 = gNode1->GetWorldTransMatrix()->TransformCoord(triangle1.v1);
01002                                           //triangle1.v2 = gNode1->GetWorldTransMatrix()->TransformCoord(triangle1.v2);
01003                                           //triangle1.v3 = gNode1->GetWorldTransMatrix()->TransformCoord(triangle1.v3);
01004 
01005                                           //Triangle triangle2 = triangleList2[iNodeId + i];
01006                                           //triangle2.v1 = gNode2->GetWorldTransMatrix()->TransformCoord(triangle2.v1);
01007                                           //triangle2.v2 = gNode2->GetWorldTransMatrix()->TransformCoord(triangle2.v2);
01008                                           //triangle2.v3 = gNode2->GetWorldTransMatrix()->TransformCoord(triangle2.v3);
01009 
01010                                           CollisionReport collisionReport1 = pDetector->FindCollision((void*)&(*it2), (void*)&(*it3));
01011 
01012                                           if (collisionReport1.bCollide
01013                                                  || (fSepareteDistance > 0 && collisionReport1.fSqueredSeparationDistance < fSepareteDistance*fSepareteDistance))
01014                                           {
01015                                                  if (collisionReport1.fSqueredSeparationDistance != -1)
01016                                                  {
01017                                                         if (collisionReport->fSqueredSeparationDistance == -1)
01018                                                         {
01019                                                                collisionReport->fSqueredSeparationDistance = collisionReport1.fSqueredSeparationDistance;
01020                                                         }
01021                                                         else if (collisionReport->fSqueredSeparationDistance > collisionReport1.fSqueredSeparationDistance)
01022                                                         {
01023                                                                collisionReport->fSqueredSeparationDistance = collisionReport1.fSqueredSeparationDistance;
01024                                                         }
01025                                                  }
01026 
01027                                                  collisionReport->bCollide = true;
01028                                                  //collisionReport.collisionTrianglesIDs
01029 
01030 
01031                                                  if (bCalculateCollisionPoint)
01032                                                  {
01033                                                         //collisionReport.collisionPoints.push_back(collisionReport1.collisionPoints[0]);
01034 
01035                                                         int iArea = assignCollisionTrianglesToAreas(&CollisionAreaList,(*it2),(*it3));
01036                                                         if (collisionReport1.collisionPoints.size() > 0)
01037                                                                CollisionAreaList[iArea].collisionPoints.push_back(collisionReport1.collisionPoints[0]);
01038 
01039                                                  }
01040 
01041                                           }
01042                                    }
01043                             }
01044 
01045                      }
01046 
01047                      delete pDetector;
01048                      delete CollisionNodes2;
01049 
01050                      if (bWriteComputeTimeToLog)
01051                      {
01052                             clock_t t2=clock();
01053                             wxString str;
01054                             str.append(wxT("<Collision_detection> Geometry test done for geometry "));
01055                             str << pNode1->GetSceneNodeID();
01056                             str.append(wxT(" with "));
01057                             str << pNode2->GetSceneNodeID();
01058 
01059                             str.append(wxT(" , time cost "));
01060 
01061                             double d = double(t2-t1);
01062 
01063                             str << d;
01064                             LOG(str);
01065                      }
01066 
01067                      //collisionReport = TestGeometryTriangles2(pGeometry1,gNode1->GetWorldTransMatrix(), pGeometry2,gNode2->GetWorldTransMatrix());
01068               }
01069 
01070               if (bCalculateCollisionPoint && CollisionAreaList.size() > 0)
01071               {
01072                      calculateCollisionPoint(&CollisionAreaList,collisionReport);
01073                      calculateCollisionRadiuses(&CollisionAreaList,collisionReport);
01074                      calculateCollisionPlane(&CollisionAreaList,collisionReport);
01075               }
01076 
01077        }
01078 
01079 }
01080 
01081 void Collision_detection::calculateCollisionPlane(std::vector<CollisionArea> *CollisionAreas, CollisionReport *report)
01082 {
01083        // Calculate collision point for every collision area
01084        for (std::vector<CollisionArea>::iterator it = CollisionAreas->begin();
01085               it != CollisionAreas->end(); it++)
01086        {
01087               VECTOR3 planeNormal;
01088 
01089               // if there are more than 2 points try to calculate the plane from combination of 3points
01090               if ((*it).collisionPoints.size() > 2)
01091               {
01092                      for (int i = 0; i < RANDOM_ALG_ITER_COUNT ; i++)
01093                      {
01094                             int a = random(0,(int)(*it).collisionPoints.size()-1);
01095                             int b = random(0,(int)(*it).collisionPoints.size()-1);
01096                             int c = random(0,(int)(*it).collisionPoints.size()-1);
01097 
01098                             VECTOR3 ab = (*it).collisionPoints[b] - (*it).collisionPoints[a];
01099                             VECTOR3 ac = (*it).collisionPoints[c] - (*it).collisionPoints[a];
01100 
01101                             if (ab.LengthSq() == 0 || ac.LengthSq() == 0)
01102                                    continue;
01103 
01104 
01105                             VECTOR3 curPlaneNormal = ab.Cross(ac);
01106                             if (curPlaneNormal.LengthSq() == 0)
01107                                    continue;
01108 
01109 
01110                             curPlaneNormal = curPlaneNormal.Normalize();
01111 
01112                             float ii = curPlaneNormal.Dot(planeNormal);
01113                             if (ii < 0)
01114                             {
01115                                    curPlaneNormal = ac.Cross(ab);
01116                             }
01117 
01118 
01119 
01120 
01121                             planeNormal += curPlaneNormal.Normalize();
01122                             planeNormal = planeNormal.Normalize();
01123                      }
01124 
01125                      // We did not find any three different points in the list
01126                      //     go through the list and try to find 1 or 2 different points
01127                      if (planeNormal.LengthSq() == 0)
01128                      {
01129                             VECTOR3 a,b;
01130                             for (int i = 0; i < (int)(*it).collisionPoints.size() ; i++)
01131                             {
01132                                    if (a.LengthSq() == 0 && (*it).collisionPoints[i].LengthSq() != 0)
01133                                    {
01134                                           // We found first unique point so take plane normal vector from its first triangle
01135                                           a = (*it).collisionPoints[i];
01136                                           VECTOR3 ab = (*it).Triangles[i*2].v2 - (*it).Triangles[i*2].v1;
01137                                           VECTOR3 ac = (*it).Triangles[i*2].v3 - (*it).Triangles[i*2].v1;
01138 
01139                                           planeNormal = ab.Cross(ac);
01140 
01141                                           planeNormal = planeNormal.Normalize();
01142 
01143                                    }
01144                                    else if (b.LengthSq() == 0 && (*it).collisionPoints[i].LengthSq() != 0 && (*it).collisionPoints[i] != a)
01145                                    {
01146                                           // We found second unique point so cmpute plane normal from last one and from triangle from this point
01147                                           b = (*it).collisionPoints[i];
01148                                           VECTOR3 ab = (*it).Triangles[i*2].v2 - (*it).Triangles[i*2].v1;
01149                                           VECTOR3 ac = (*it).Triangles[i*2].v3 - (*it).Triangles[i*2].v1;
01150 
01151                                           VECTOR3 curPlaneNormal = ab.Cross(ac);
01152 
01153                                           planeNormal += curPlaneNormal.Normalize();
01154                                           planeNormal = planeNormal.Normalize();
01155 
01156 
01157                                           break;
01158                                    }
01159                             }
01160 
01161                      }
01162 
01163               }
01164 
01165               report->collisionPlaneNormals.push_back(planeNormal);
01166        }
01167 
01168 
01169 
01170 }
01171 
01172 void Collision_detection::calculateCollisionRadiuses(std::vector<CollisionArea> *CollisionAreas, CollisionReport *report)
01173 {
01174        // Calculate collision point for every collision area
01175        std::vector<VECTOR3>::iterator it2;
01176        for (int i = 0; i < (int)CollisionAreas->size(); i++)
01177        {
01178               VECTOR3 collisionPoint = report->collisionPoints[i];
01179 
01180               float x,y,z;
01181               x = y = z = 0;
01182 
01183               for (std::vector<VECTOR3>::iterator it2 = (*CollisionAreas)[i].collisionPoints.begin();
01184                      it2 != (*CollisionAreas)[i].collisionPoints.end(); it2++)
01185               {
01186                      float _x = abs((*it2).x - collisionPoint.x);
01187                      float _y = abs((*it2).y - collisionPoint.y);
01188                      float _z = abs((*it2).z - collisionPoint.z);
01189 
01190 
01191                      x = __max(_x,x);
01192                      y = __max(_y,y);
01193                      z = __max(_z,z);
01194               }
01195 
01196               float radius = __max(x,y);
01197               radius = __max(z,radius);
01198 
01199               report->collisionRadiuses.push_back(radius);
01200        }
01201 
01202 
01203 
01204 }
01205 
01206 void Collision_detection::calculateCollisionPoint(std::vector<CollisionArea> *CollisionAreas, CollisionReport *report)
01207 {
01208        // Calculate collision point for every collision area
01209        for (std::vector<CollisionArea>::iterator it = CollisionAreas->begin();
01210               it != CollisionAreas->end(); it++)
01211        {
01212               VECTOR3 collisionPoint;
01213               float x,y,z;
01214               x = y = z = 0;
01215 
01216               for (std::vector<VECTOR3>::iterator it2 = (*it).collisionPoints.begin();
01217                      it2 != (*it).collisionPoints.end(); it2++)
01218               {
01219                      x += (*it2).x;
01220                      y += (*it2).y;
01221                      z += (*it2).z;
01222               }
01223 
01224               x = x / (*it).collisionPoints.size();
01225               y = y / (*it).collisionPoints.size();
01226               z = z / (*it).collisionPoints.size();
01227 
01228               report->collisionPoints.push_back(VECTOR3(x,y,z));
01229        }
01230 
01231 
01232 
01233 }
01234 
01235 
01236 int Collision_detection::assignCollisionTrianglesToAreas(std::vector<CollisionArea> *CollisionAreas, Triangle tr1, Triangle tr2)
01237 {
01238        // Find area conected to one of the given triangles
01239        bool bFound = false;
01240        int iArea = -1;
01241 
01242        for (std::vector<CollisionArea>::iterator it = CollisionAreas->begin();
01243               it != CollisionAreas->end(); it++)
01244        {
01245               iArea++;
01246 
01247               for (std::vector<Triangle>::iterator it2 = (*it).Triangles.begin();
01248                      it2 != (*it).Triangles.end(); it2++)
01249               {
01250                      for (int i = 0; i < 3; i++)
01251                      {
01252                             if ( (*it2)[i].x == tr1[i].x || (*it2)[i].x == tr2[i].x
01253                                    ||(*it2)[i].y == tr1[i].y || (*it2)[i].y == tr2[i].y
01254                                    ||(*it2)[i].z == tr1[i].z || (*it2)[i].z == tr2[i].z)
01255                             {
01256                                    bFound = true;
01257                                    it->Triangles.push_back(tr1);
01258                                    it->Triangles.push_back(tr2);
01259                                    break;
01260                             }
01261                      }
01262                      if (bFound == true)
01263                             break;
01264               }
01265               if (bFound == true)
01266                      break;
01267 
01268        }
01269 
01270        // Triangles are not connected to any current areas, create new area
01271        if (!bFound)
01272        {
01273               iArea++;
01274               CollisionArea newArea;
01275               newArea.Triangles.push_back(tr1);
01276               newArea.Triangles.push_back(tr2);
01277 
01278               CollisionAreas->push_back(newArea);
01279        }
01280 
01281        return iArea;
01282 
01283 }
01284 
01285 void Collision_detection::reportCollision(const BVHNode * pNode1, const BVHNode * pNode2)
01286 {
01287        wxString s = wxT("<Collision_detection> Collision found between");
01288        s << pNode1->GetSceneNodeID();
01289        s << wxT(" and ");
01290        s << pNode2->GetSceneNodeID();
01291 
01292        LOG(s);
01293 }
01294 
01295 
01296 //CollisionReport Collision_detection::TestPrimitives9(const BVHNode * pNode1, const BVHNode * pNode2)
01297 //{
01298 //     Scene* pScene = GetSceneMgr()->GetScene(sceneID);
01299 //
01300 //
01301 //     CollisionReport collisionReport;
01302 //     collisionReport.bCollide = false;
01303 //
01304 //     //clock_t t1,t2;
01305 //
01306 //
01307 //
01308 //     if (pScene)
01309 //     {
01310 //            GeometryNode* gNode1 = (GeometryNode*)pScene->GetNode(pNode1->GetSceneNodeID());
01311 //            const Geometry* pGeometry1 = pScene->GetGeometry(gNode1->GetGeometryID());
01312 //
01313 //            GeometryNode* gNode2 = (GeometryNode*)pScene->GetNode(pNode2->GetSceneNodeID());
01314 //            const Geometry* pGeometry2 = pScene->GetGeometry(gNode2->GetGeometryID());
01315 //
01316 //            if (pGeometry1->type == Geometry::GEOMETRY_TRIANGLE
01317 //                   && pGeometry2->type == Geometry::GEOMETRY_TRIANGLE)
01318 //            {
01319 //                   std::vector<Triangle> triangleList1 = pGeometry1->Triangulate();
01320 //                   std::vector<Triangle> triangleList2 = pGeometry2->Triangulate();
01321 //
01322 //                   std::map<NODE_ID,Bvh*>::iterator it1 = mapOfBVHs9.find(pNode1->GetSceneNodeID());
01323 //                   if (it1 == mapOfBVHs9.end())
01324 //                   {
01325 //
01326 //                          Bvh* bvh = new Bvh(triangleList1);
01327 //                          //t1=clock();
01328 //
01329 //
01330 //
01331 //                          bvh->Construct();// Build(&triangleList1,gNode1->GetWorldTransMatrix());
01332 //                          bvh->UpdateBoxes(bvh->root);// UpdateBV(NULL);
01333 //
01334 //
01335 //                          //t2=clock();
01336 //                          //wxString str;
01337 //                          //str.append(wxT("<Collision_detection> Built BVH for node "));
01338 //                          //str << pNode1->GetSceneNodeID();
01339 //                          //str.append(wxT(" with "));
01340 //                          //str << (unsigned int)triangleList1.size();
01341 //
01342 //                          //str.append(wxT(" triangles, time cost "));
01343 //
01344 //                          //double d = double(t2-t1);
01345 //
01346 //                          //str << d;
01347 //
01348 //                          //LOG(str);
01349 //                          mapOfBVHs9.insert(SAH_BVH_Pair(pNode1->GetSceneNodeID(),bvh));
01350 //                          it1 = mapOfBVHs9.find(pNode1->GetSceneNodeID());
01351 //                   }
01352 //
01353 //                   std::map<NODE_ID,Bvh*>::iterator it2 = mapOfBVHs9.find(pNode2->GetSceneNodeID());
01354 //                   if (it2 == mapOfBVHs9.end())
01355 //                   {
01356 //
01357 //                          //t1=clock();
01358 //
01359 //                          Bvh* bvh2 = new Bvh(triangleList2);
01360 //
01361 //                          bvh2->Construct();// Build(&triangleList1,gNode1->GetWorldTransMatrix());
01362 //                          bvh2->UpdateBoxes(bvh2->root);// UpdateBV(NULL);
01363 //
01364 //
01365 //
01366 //
01367 //                          //t2=clock();
01368 //                          //wxString str;
01369 //                          //str.append(wxT("<Collision_detection> Built BVH for node "));
01370 //                          //str << pNode2->GetSceneNodeID();
01371 //                          //str.append(wxT(" with "));
01372 //                          //str << (unsigned int)triangleList2.size();
01373 //
01374 //                          //str.append(wxT(" triangles, time cost "));
01375 //
01376 //                          //double d = double(t2-t1);
01377 //
01378 //                          //str << d;
01379 //                          //LOG(str);
01380 //
01381 //                          mapOfBVHs9.insert(SAH_BVH_Pair(pNode2->GetSceneNodeID(),bvh2));
01382 //
01383 //                          it2 = mapOfBVHs9.find(pNode2->GetSceneNodeID());
01384 //
01385 //                   }
01386 //
01387 //
01388 //                   //t1=clock();
01389 //
01390 //                   std::vector<SAH_CollisionPair>* CollisionNodes = new std::vector<SAH_CollisionPair>();
01391 //
01392 //                   findIntersection9((*it1).second->root,(*it2).second->root,CollisionNodes);
01393 //
01394 //
01395 //
01396 //
01397 //                   Collison_Detector_IFace* pDetector = CollisionDetector::GetColisionDetector(CollisionDetector::DT_Triangle);
01398 //
01399 //                   //wxString str1;
01400 //                   //str1.append(wxT("<Collision_detection> After traversing BVH "));
01401 //                   //str1 << (int)CollisionNodes->size();
01402 //                   //str1.append(wxT(" collision pairs has been found "));
01403 //                   //LOG(str1);
01404 //
01405 //
01406 //
01407 //                   for (std::vector<SAH_CollisionPair>::iterator it5 = CollisionNodes->begin();
01408 //                                 it5 != CollisionNodes->end(); it5++)
01409 //                   {
01410 //                          int Node1First = ((BvhLeaf*)(*it5).pNodeL)->first;
01411 //                          int Node1Last = ((BvhLeaf*)(*it5).pNodeL)->last;
01412 //                          int Node2First = ((BvhLeaf*)(*it5).pNodeR)->first;
01413 //                          int Node2Last = ((BvhLeaf*)(*it5).pNodeR)->last;
01414 //
01415 //                          for (int i = Node1First; i < Node1Last; i++)
01416 //                          {
01417 //                                 for (int a = Node2First; a < Node2Last; a++)
01418 //                                 {
01419 //                                        //Triangle triangle1 = triangleList1[iNodeId + i];
01420 //                                        //triangle1.v1 = gNode1->GetWorldTransMatrix()->TransformCoord(triangle1.v1);
01421 //                                        //triangle1.v2 = gNode1->GetWorldTransMatrix()->TransformCoord(triangle1.v2);
01422 //                                        //triangle1.v3 = gNode1->GetWorldTransMatrix()->TransformCoord(triangle1.v3);
01423 //
01424 //                                        //Triangle triangle2 = triangleList2[iNodeId + i];
01425 //                                        //triangle2.v1 = gNode2->GetWorldTransMatrix()->TransformCoord(triangle2.v1);
01426 //                                        //triangle2.v2 = gNode2->GetWorldTransMatrix()->TransformCoord(triangle2.v2);
01427 //                                        //triangle2.v3 = gNode2->GetWorldTransMatrix()->TransformCoord(triangle2.v3);
01428 //
01429 //                                        CollisionReport collisionReport1 = pDetector->FindCollision((void*)&(*it1).second->triangles[i], (void*)&(*it2).second->triangles[a]);
01430 //
01431 //                                        if (collisionReport1.bCollide)
01432 //                                        {
01433 //                                               collisionReport.bCollide = true;
01434 //                                               //collisionReport.collisionTrianglesIDs
01435 //
01436 //                                        }
01437 //                                 }
01438 //                          }
01439 //
01440 //                   }
01441 //
01442 //
01443 //
01444 //                   //t2=clock();
01445 //                   //wxString str;
01446 //                   //str.append(wxT("<Collision_detection> Geometry test done for geometry "));
01447 //                   //str << pNode1->GetSceneNodeID();
01448 //                   //str.append(wxT(" with "));
01449 //                   //str << pNode2->GetSceneNodeID();
01450 //
01451 //                   //str.append(wxT(" , time cost "));
01452 //
01453 //                   //double d = double(t2-t1);
01454 //
01455 //                   //str << d;
01456 //                   //LOG(str);
01457 //
01458 //            }
01459 //     }
01460 //
01461 //     return collisionReport;
01462 //}
01463 
01464 //CollisionReport Collision_detection::TestGeometryTriangles2(const Geometry* pGeometry1, const MATRIX* transfMatrix1,
01465 //                                                                                  const Geometry* pGeometry2, const MATRIX* transfMatrix2)
01466 //{
01467 //     CollisionReport collisionReport;
01468 //     collisionReport.bCollide = false;
01469 //
01470 //                          std::vector<Triangle> triangleList1 = pGeometry1->Triangulate();
01471 //                          std::vector<Triangle> triangleList2 = pGeometry2->Triangulate();
01472 //
01473 //     CollisionDetection_BVH* bvh = new CollisionDetection_BVH();
01474 //
01475 //     bvh->Build(&triangleList1,transfMatrix1);
01476 //     bvh->UpdateBV(bvh->root);
01477 //
01478 //
01479 //     CollisionDetection_BVH* bvh2 = new CollisionDetection_BVH();
01480 //
01481 //     bvh2->Build(&triangleList2,transfMatrix2);
01482 //     bvh2->UpdateBV(bvh2->root);
01483 //
01484 //     std::vector<CollisionPair>* CollisionNodes = new std::vector<CollisionPair>();
01485 //     findIntersection3(bvh->root,bvh2->root,CollisionNodes);
01486 //
01487 //
01488 //
01489 //     for (std::vector<CollisionPair>::iterator it = CollisionNodes->begin();it != CollisionNodes->end(); )
01490 //     {
01491 //            Triangle triangle1 = triangleList1[(*it).pNodeL->GetSceneNodeID()];
01492 //            Triangle triangle2 = triangleList1[(*it++).pNodeR->GetSceneNodeID()];
01493 //
01494 //            collisionReport = CollisionDetector::DetectCollision((void*)&triangle1, (void*)&triangle2, CollisionDetector::DT_Triangle_Distance);
01495 //
01496 //
01497 //     }
01498 //
01499 //     return collisionReport;
01500 //}
01501 
01502 
01503 
01504 
01505 
01506 
01507
/cygdrive/d/src/svn/vrut/trunk/modules/collision_detection/collision_detection.cpp
