ColdetModel.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  */
00013 #include <iostream>
00014 #include "ColdetModel.h"
00015 #include "ColdetModelSharedDataSet.h"
00016 
00017 #include "Opcode/Opcode.h"
00018 
00019 
00020 using namespace std;
00021 using namespace hrp;
00022 
00023 
00024 ColdetModel::ColdetModel()
00025 {
00026     dataSet = new ColdetModelSharedDataSet();
00027     isValid_ = false;
00028     initialize();
00029     dataSet->neighbor.clear();
00030     triangleMap.clear();
00031 }
00032 
00033 
00034 ColdetModel::ColdetModel(const ColdetModel& org)
00035     : name_(org.name_),
00036       isValid_(org.isValid_)
00037 {
00038     dataSet = org.dataSet;
00039     initialize();
00040 }
00041 
00042 
00043 void ColdetModel::initialize()
00044 {
00045     dataSet->refCounter++;
00046 
00047     transform = new IceMaths::Matrix4x4();
00048     transform->Identity();
00049 
00050     pTransform = new IceMaths::Matrix4x4();
00051     pTransform->Identity();
00052 }
00053 
00054 
00055 ColdetModelSharedDataSet::ColdetModelSharedDataSet()
00056 {
00057     refCounter = 0;
00058     pType = ColdetModel::SP_MESH;
00059     AABBTreeMaxDepth=0;
00060 }    
00061 
00062 
00063 ColdetModel::~ColdetModel()
00064 {
00065     if(--dataSet->refCounter <= 0){
00066         delete dataSet;
00067     }
00068     delete pTransform;
00069     delete transform;
00070 }
00071 
00072 
00073 void ColdetModel::setNumVertices(int n)
00074 {
00075     dataSet->vertices.resize(n);
00076 }
00077 
00078 
00079 int ColdetModel::getNumVertices() const
00080 {
00081     return dataSet->vertices.size();
00082 }
00083 
00084 
00085 void ColdetModel::setNumTriangles(int n)
00086 {
00087     dataSet->triangles.resize(n);
00088     initNeighbor(n);
00089 }
00090 
00091 
00092 int ColdetModel::getNumTriangles() const
00093 {
00094     return dataSet->triangles.size();
00095 }
00096 
00097         
00098 void ColdetModel::setVertex(int index, float x, float y, float z)
00099 {
00100     dataSet->vertices[index].Set(x, y, z);
00101 }
00102 
00103 
00104 void ColdetModel::addVertex(float x, float y, float z)
00105 {
00106     dataSet->vertices.push_back(IceMaths::Point(x, y, z));
00107 }
00108 
00109         
00110 void ColdetModel::getVertex(int index, float& x, float& y, float& z) const
00111 {
00112     const Point& v = dataSet->vertices[index];
00113     x = v.x;
00114     y = v.y;
00115     z = v.z;
00116 }
00117 
00118         
00119 void ColdetModel::setTriangle(int index, int v1, int v2, int v3)
00120 {
00121     udword* mVRef = dataSet->triangles[index].mVRef;
00122     mVRef[0] = v1;
00123     mVRef[1] = v2;
00124     mVRef[2] = v3;
00125     setNeighborTriangle(index, v1, v2, v3);
00126 }
00127 
00128 void ColdetModel::getTriangle(int index, int& v1, int& v2, int& v3) const
00129 {
00130     udword* mVRef = dataSet->triangles[index].mVRef;
00131     v1=mVRef[0];
00132     v2=mVRef[1];
00133     v3=mVRef[2];
00134 }
00135 
00136 
00137 void ColdetModel::addTriangle(int v1, int v2, int v3)
00138 {
00139     dataSet->triangles.push_back(IceMaths::IndexedTriangle(v1, v2, v3));
00140 }
00141 
00142 
00143 void ColdetModel::build()
00144 {
00145     isValid_ = dataSet->build();
00146     /*
00147     unsigned int maxDepth = dataSet->getAABBTreeDepth();
00148     for(unsigned int i=0; i<maxDepth; i++){
00149        vector<IceMaths::Point> data = getBoundingBoxData(i);   
00150        cout << "depth= " << i << endl;
00151        for(vector<IceMaths::Point>::iterator it=data.begin(); it!=data.end(); it++){
00152             cout << (*it).x << " " << (*it).y << " " << (*it).z << endl;
00153        }
00154    }
00155    */
00156 }
00157 
00158 
00159 bool ColdetModelSharedDataSet::build()
00160 {
00161     bool result = false;
00162     
00163     if(triangles.size() > 0){
00164 
00165         Opcode::OPCODECREATE OPCC;
00166 
00167         iMesh.SetPointers(&triangles[0], &vertices[0]);
00168         iMesh.SetNbTriangles(triangles.size());
00169         iMesh.SetNbVertices(vertices.size());
00170         
00171         OPCC.mIMesh = &iMesh;
00172         
00173         OPCC.mNoLeaf = false;
00174         OPCC.mQuantized = false;
00175         OPCC.mKeepOriginal = false;
00176         
00177         model.Build(OPCC);
00178         if(model.GetTree()){
00179             AABBTreeMaxDepth = computeDepth(((Opcode::AABBCollisionTree*)model.GetTree())->GetNodes(), 0, -1) + 1;
00180             for(int i=0; i<AABBTreeMaxDepth; i++)
00181                 for(int j=0; j<i; j++)
00182                     numBBMap.at(i) += numLeafMap.at(j);
00183         }
00184         result = true;
00185     }
00186 
00187     return result;
00188 }
00189 
00190 int ColdetModel::numofBBtoDepth(int minNumofBB){
00191     for(int i=0; i<getAABBTreeDepth(); i++)
00192         if(minNumofBB <= dataSet->getNumofBB(i))
00193             return i;
00194     return getAABBTreeDepth();
00195 }
00196 
00197 int ColdetModel::getAABBTreeDepth(){
00198     return dataSet->getAABBTreeDepth();
00199 }
00200 
00201 int ColdetModel::getAABBmaxNum(){
00202     return dataSet->getmaxNumofBB();
00203 }
00204 
00205 
00206 static void getBoundingBoxDataSub
00207 (const Opcode::AABBCollisionNode* node, unsigned int currentDepth, unsigned int depth, std::vector<Vector3>& out_data){
00208     if(currentDepth == depth || node->IsLeaf() ){
00209         const IceMaths::Point& p = node->mAABB.mCenter;
00210         out_data.push_back(Vector3(p.x, p.y, p.z));
00211         const IceMaths::Point& q = node->mAABB.mExtents;
00212         out_data.push_back(Vector3(q.x, q.y, q.z));
00213     }
00214     currentDepth++;
00215     if(currentDepth > depth) return;
00216     if(!node->IsLeaf()){
00217         getBoundingBoxDataSub(node->GetPos(), currentDepth, depth, out_data);
00218         getBoundingBoxDataSub(node->GetNeg(), currentDepth, depth, out_data);
00219     }
00220 }
00221 
00222 
00223 void ColdetModel::getBoundingBoxData(const int depth, std::vector<Vector3>& out_data){
00224     const Opcode::AABBCollisionNode* rootNode=((Opcode::AABBCollisionTree*)dataSet->model.GetTree())->GetNodes();
00225     out_data.clear();
00226     getBoundingBoxDataSub(rootNode, 0, depth, out_data);
00227 }
00228 
00229 
00230 int ColdetModelSharedDataSet::computeDepth(const Opcode::AABBCollisionNode* node, int currentDepth, int max )
00231 {
00232     /*
00233         cout << "depth= " << currentDepth << " ";
00234     Point p = node->mAABB.mCenter;
00235     cout << p.x << " " << p.y << " " << p.z << "     ";
00236     p = node->mAABB.mExtents;
00237     cout << p.x << " " << p.y << " " << p.z << " ";
00238     if(node->IsLeaf()) cout << "is Leaf " ;
00239     cout << endl;
00240     */
00241     if(max < currentDepth){
00242         max = currentDepth;
00243         numBBMap.push_back(0);
00244         numLeafMap.push_back(0);
00245     }
00246     numBBMap.at(currentDepth)++;
00247 
00248     if(!node->IsLeaf()){
00249         currentDepth++;
00250         max = computeDepth(node->GetPos(), currentDepth, max);
00251         max = computeDepth(node->GetNeg(), currentDepth, max);
00252     }else
00253         numLeafMap.at(currentDepth)++;
00254 
00255     return max;
00256 }
00257 
00258 void ColdetModel::setPosition(const Matrix33& R, const Vector3& p)
00259 {
00260     transform->Set((float)R(0,0), (float)R(1,0), (float)R(2,0), 0.0f,
00261                    (float)R(0,1), (float)R(1,1), (float)R(2,1), 0.0f,
00262                    (float)R(0,2), (float)R(1,2), (float)R(2,2), 0.0f,
00263                    (float)p(0),   (float)p(1),   (float)p(2),   1.0f);
00264 }
00265 
00266 
00267 void ColdetModel::setPosition(const double* R, const double* p)
00268 {
00269     transform->Set((float)R[0], (float)R[3], (float)R[6], 0.0f,
00270                    (float)R[1], (float)R[4], (float)R[7], 0.0f,
00271                    (float)R[2], (float)R[5], (float)R[8], 0.0f,
00272                    (float)p[0], (float)p[1], (float)p[2], 1.0f);
00273 }
00274 
00275 void ColdetModel::setPrimitiveType(PrimitiveType ptype)
00276 {
00277     dataSet->pType = ptype;
00278 }
00279 
00280 ColdetModel::PrimitiveType ColdetModel::getPrimitiveType() const
00281 {
00282     return dataSet->pType;
00283 }
00284 
00285 void ColdetModel::setNumPrimitiveParams(unsigned int nparam)
00286 {
00287     dataSet->pParams.resize(nparam);
00288 }
00289 
00290 bool ColdetModel::setPrimitiveParam(unsigned int index, float value)
00291 {
00292     if (index >= dataSet->pParams.size()) return false;
00293 
00294     dataSet->pParams[index] = value;
00295     return true;
00296 }
00297 
00298 bool ColdetModel::getPrimitiveParam(unsigned int index, float& value) const
00299 {
00300     if (index >= dataSet->pParams.size()) return false;
00301 
00302     value = dataSet->pParams[index];
00303     return true;
00304 }
00305 
00306 void ColdetModel::setPrimitivePosition(const double* R, const double* p)
00307 {
00308     pTransform->Set((float)R[0], (float)R[3], (float)R[6], 0.0f,
00309                     (float)R[1], (float)R[4], (float)R[7], 0.0f,
00310                     (float)R[2], (float)R[5], (float)R[8], 0.0f,
00311                     (float)p[0], (float)p[1], (float)p[2], 1.0f);
00312 }
00313 
00314 double ColdetModel::computeDistanceWithRay(const double *point, 
00315                                            const double *dir)
00316 {
00317     Opcode::RayCollider RC;
00318     Ray world_ray(Point(point[0], point[1], point[2]),
00319                   Point(dir[0], dir[1], dir[2]));
00320     Opcode::CollisionFace CF;
00321     Opcode::SetupClosestHit(RC, CF);
00322     udword Cache;
00323     RC.Collide(world_ray, dataSet->model, transform, &Cache);
00324     if (CF.mDistance == FLT_MAX){
00325         return 0;
00326     }else{
00327         return CF.mDistance;
00328     }
00329 }
00330 
00331 bool ColdetModel::checkCollisionWithPointCloud(const std::vector<Vector3> &i_cloud, double i_radius)
00332 {
00333     Opcode::SphereCollider SC;
00334     SC.SetFirstContact(true);
00335     Opcode::SphereCache Cache;
00336     IceMaths::Point p(0,0,0);
00337     IceMaths::Sphere sphere(p, i_radius);
00338     IceMaths::Matrix4x4 sphereTrans(1,0,0,0, 0,1,0,0, 0,0,1,0,  0,0,0,1);
00339     for (unsigned int i=0; i<i_cloud.size(); i++){
00340         const Vector3& p = i_cloud[i];
00341         sphereTrans.m[3][0] = p[0];
00342         sphereTrans.m[3][1] = p[1];
00343         sphereTrans.m[3][2] = p[2];
00344         bool isOk = SC.Collide(Cache, sphere, dataSet->model, &sphereTrans, transform); 
00345         if (!isOk) std::cerr << "SphereCollider::Collide() failed" << std::endl;
00346         if (SC.GetContactStatus()) return true;
00347     }
00348     return false;
00349 }
00350 
00351 void ColdetModel::setNeighborTriangle(int triangle, int vertex0, int vertex1, int vertex2){
00352     setNeighborTriangleSub(triangle, vertex0, vertex1);
00353     setNeighborTriangleSub(triangle, vertex1, vertex2);
00354     setNeighborTriangleSub(triangle, vertex2, vertex0);
00355 }
00356 
00357 bool ColdetModel::setNeighborTriangleSub(int triangle, int vertex0, int vertex1){
00358     Edge edge(vertex0, vertex1);
00359     EdgeToTriangleMap::iterator p = triangleMap.find(edge);
00360     if(p == triangleMap.end()){
00361         triangleMap[edge].t[0] = triangle;
00362         triangleMap[edge].t[1] = -1;
00363         return true;
00364     } else {
00365         trianglePair& triangles = p->second;
00366         if( triangles.t[1] != -1 ){
00367             dataSet->neighbor[triangles.t[0]].deleteNeighbor(triangles.t[1]);
00368             dataSet->neighbor[triangles.t[1]].deleteNeighbor(triangles.t[0]);
00369             //cout << "neighbors[" << triangles.t[0] << "] " << neighbors[triangles.t[0]][0] << " " << neighbors[triangles.t[0]][1] << " " << neighbors[triangles.t[0]][2] << endl;
00370             //cout << "neighbors[" << triangles.t[1] << "] " << neighbors[triangles.t[1]][0] << " " << neighbors[triangles.t[1]][1] << " " << neighbors[triangles.t[1]][2] << endl;
00371             return false;
00372         }else{
00373             dataSet->neighbor[triangle].addNeighbor(triangles.t[0]);
00374             dataSet->neighbor[triangles.t[0]].addNeighbor(triangle);
00375             triangles.t[1] = triangle;
00376             //cout << "neighbors[" << triangle << "] " << neighbors[triangle][0] << " " << neighbors[triangle][1] << " " << neighbors[triangle][2] << endl;
00377             //cout << "neighbors[" << triangles.t[0] << "] " << neighbors[triangles.t[0]][0] << " " << neighbors[triangles.t[0]][1] << " " << neighbors[triangles.t[0]][2] << endl;
00378             return true;
00379         }
00380     }
00381 }
00382 
00383 void ColdetModel::initNeighbor(int n){
00384     dataSet->neighbor.resize(n);
00385 }
00386 
00387 void ColdetModel::setNeighbor(int triangle0, int triangle1 ){
00388 
00389 }
00390 


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