ColdetModel.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  */
13 #include <iostream>
14 #include "ColdetModel.h"
16 
17 #include "Opcode/Opcode.h"
18 
19 
20 using namespace std;
21 using namespace hrp;
22 
23 
24 ColdetModel::ColdetModel()
25 {
26  dataSet = new ColdetModelSharedDataSet();
27  isValid_ = false;
28  initialize();
29  dataSet->neighbor.clear();
30  triangleMap.clear();
31 }
32 
33 
34 ColdetModel::ColdetModel(const ColdetModel& org)
35  : name_(org.name_),
36  isValid_(org.isValid_)
37 {
38  dataSet = org.dataSet;
39  initialize();
40 }
41 
42 
44 {
46 
47  transform = new IceMaths::Matrix4x4();
48  transform->Identity();
49 
50  pTransform = new IceMaths::Matrix4x4();
51  pTransform->Identity();
52 }
53 
54 
56 {
57  refCounter = 0;
60 }
61 
62 
64 {
65  if(--dataSet->refCounter <= 0){
66  delete dataSet;
67  }
68  delete pTransform;
69  delete transform;
70 }
71 
72 
74 {
75  dataSet->vertices.resize(n);
76 }
77 
78 
80 {
81  return dataSet->vertices.size();
82 }
83 
84 
86 {
87  dataSet->triangles.resize(n);
88  initNeighbor(n);
89 }
90 
91 
93 {
94  return dataSet->triangles.size();
95 }
96 
97 
98 void ColdetModel::setVertex(int index, float x, float y, float z)
99 {
100  dataSet->vertices[index].Set(x, y, z);
101 }
102 
103 
104 void ColdetModel::addVertex(float x, float y, float z)
105 {
106  dataSet->vertices.push_back(IceMaths::Point(x, y, z));
107 }
108 
109 
110 void ColdetModel::getVertex(int index, float& x, float& y, float& z) const
111 {
112  const Point& v = dataSet->vertices[index];
113  x = v.x;
114  y = v.y;
115  z = v.z;
116 }
117 
118 
119 void ColdetModel::setTriangle(int index, int v1, int v2, int v3)
120 {
121  udword* mVRef = dataSet->triangles[index].mVRef;
122  mVRef[0] = v1;
123  mVRef[1] = v2;
124  mVRef[2] = v3;
125  setNeighborTriangle(index, v1, v2, v3);
126 }
127 
128 void ColdetModel::getTriangle(int index, int& v1, int& v2, int& v3) const
129 {
130  udword* mVRef = dataSet->triangles[index].mVRef;
131  v1=mVRef[0];
132  v2=mVRef[1];
133  v3=mVRef[2];
134 }
135 
136 
137 void ColdetModel::addTriangle(int v1, int v2, int v3)
138 {
139  dataSet->triangles.push_back(IceMaths::IndexedTriangle(v1, v2, v3));
140 }
141 
142 
144 {
145  isValid_ = dataSet->build();
146  /*
147  unsigned int maxDepth = dataSet->getAABBTreeDepth();
148  for(unsigned int i=0; i<maxDepth; i++){
149  vector<IceMaths::Point> data = getBoundingBoxData(i);
150  cout << "depth= " << i << endl;
151  for(vector<IceMaths::Point>::iterator it=data.begin(); it!=data.end(); it++){
152  cout << (*it).x << " " << (*it).y << " " << (*it).z << endl;
153  }
154  }
155  */
156 }
157 
158 
160 {
161  bool result = false;
162 
163  if(triangles.size() > 0){
164 
165  Opcode::OPCODECREATE OPCC;
166 
167  iMesh.SetPointers(&triangles[0], &vertices[0]);
168  iMesh.SetNbTriangles(triangles.size());
169  iMesh.SetNbVertices(vertices.size());
170 
171  OPCC.mIMesh = &iMesh;
172 
173  OPCC.mNoLeaf = false;
174  OPCC.mQuantized = false;
175  OPCC.mKeepOriginal = false;
176 
177  model.Build(OPCC);
178  if(model.GetTree()){
179  AABBTreeMaxDepth = computeDepth(((Opcode::AABBCollisionTree*)model.GetTree())->GetNodes(), 0, -1) + 1;
180  for(int i=0; i<AABBTreeMaxDepth; i++)
181  for(int j=0; j<i; j++)
182  numBBMap.at(i) += numLeafMap.at(j);
183  }
184  result = true;
185  }
186 
187  return result;
188 }
189 
190 int ColdetModel::numofBBtoDepth(int minNumofBB){
191  for(int i=0; i<getAABBTreeDepth(); i++)
192  if(minNumofBB <= dataSet->getNumofBB(i))
193  return i;
194  return getAABBTreeDepth();
195 }
196 
198  return dataSet->getAABBTreeDepth();
199 }
200 
202  return dataSet->getmaxNumofBB();
203 }
204 
205 
206 static void getBoundingBoxDataSub
207 (const Opcode::AABBCollisionNode* node, unsigned int currentDepth, unsigned int depth, std::vector<Vector3>& out_data){
208  if(currentDepth == depth || node->IsLeaf() ){
209  const IceMaths::Point& p = node->mAABB.mCenter;
210  out_data.push_back(Vector3(p.x, p.y, p.z));
211  const IceMaths::Point& q = node->mAABB.mExtents;
212  out_data.push_back(Vector3(q.x, q.y, q.z));
213  }
214  currentDepth++;
215  if(currentDepth > depth) return;
216  if(!node->IsLeaf()){
217  getBoundingBoxDataSub(node->GetPos(), currentDepth, depth, out_data);
218  getBoundingBoxDataSub(node->GetNeg(), currentDepth, depth, out_data);
219  }
220 }
221 
222 
223 void ColdetModel::getBoundingBoxData(const int depth, std::vector<Vector3>& out_data){
224  const Opcode::AABBCollisionNode* rootNode=((Opcode::AABBCollisionTree*)dataSet->model.GetTree())->GetNodes();
225  out_data.clear();
226  getBoundingBoxDataSub(rootNode, 0, depth, out_data);
227 }
228 
229 
230 int ColdetModelSharedDataSet::computeDepth(const Opcode::AABBCollisionNode* node, int currentDepth, int max )
231 {
232  /*
233  cout << "depth= " << currentDepth << " ";
234  Point p = node->mAABB.mCenter;
235  cout << p.x << " " << p.y << " " << p.z << " ";
236  p = node->mAABB.mExtents;
237  cout << p.x << " " << p.y << " " << p.z << " ";
238  if(node->IsLeaf()) cout << "is Leaf " ;
239  cout << endl;
240  */
241  if(max < currentDepth){
242  max = currentDepth;
243  numBBMap.push_back(0);
244  numLeafMap.push_back(0);
245  }
246  numBBMap.at(currentDepth)++;
247 
248  if(!node->IsLeaf()){
249  currentDepth++;
250  max = computeDepth(node->GetPos(), currentDepth, max);
251  max = computeDepth(node->GetNeg(), currentDepth, max);
252  }else
253  numLeafMap.at(currentDepth)++;
254 
255  return max;
256 }
257 
258 void ColdetModel::setPosition(const Matrix33& R, const Vector3& p)
259 {
260  transform->Set((float)R(0,0), (float)R(1,0), (float)R(2,0), 0.0f,
261  (float)R(0,1), (float)R(1,1), (float)R(2,1), 0.0f,
262  (float)R(0,2), (float)R(1,2), (float)R(2,2), 0.0f,
263  (float)p(0), (float)p(1), (float)p(2), 1.0f);
264 }
265 
266 
267 void ColdetModel::setPosition(const double* R, const double* p)
268 {
269  transform->Set((float)R[0], (float)R[3], (float)R[6], 0.0f,
270  (float)R[1], (float)R[4], (float)R[7], 0.0f,
271  (float)R[2], (float)R[5], (float)R[8], 0.0f,
272  (float)p[0], (float)p[1], (float)p[2], 1.0f);
273 }
274 
276 {
277  dataSet->pType = ptype;
278 }
279 
281 {
282  return dataSet->pType;
283 }
284 
285 void ColdetModel::setNumPrimitiveParams(unsigned int nparam)
286 {
287  dataSet->pParams.resize(nparam);
288 }
289 
290 bool ColdetModel::setPrimitiveParam(unsigned int index, float value)
291 {
292  if (index >= dataSet->pParams.size()) return false;
293 
294  dataSet->pParams[index] = value;
295  return true;
296 }
297 
298 bool ColdetModel::getPrimitiveParam(unsigned int index, float& value) const
299 {
300  if (index >= dataSet->pParams.size()) return false;
301 
302  value = dataSet->pParams[index];
303  return true;
304 }
305 
306 void ColdetModel::setPrimitivePosition(const double* R, const double* p)
307 {
308  pTransform->Set((float)R[0], (float)R[3], (float)R[6], 0.0f,
309  (float)R[1], (float)R[4], (float)R[7], 0.0f,
310  (float)R[2], (float)R[5], (float)R[8], 0.0f,
311  (float)p[0], (float)p[1], (float)p[2], 1.0f);
312 }
313 
314 double ColdetModel::computeDistanceWithRay(const double *point,
315  const double *dir)
316 {
317  Opcode::RayCollider RC;
318  Ray world_ray(Point(point[0], point[1], point[2]),
319  Point(dir[0], dir[1], dir[2]));
320  Opcode::CollisionFace CF;
321  Opcode::SetupClosestHit(RC, CF);
322  udword Cache;
323  RC.Collide(world_ray, dataSet->model, transform, &Cache);
324  if (CF.mDistance == FLT_MAX){
325  return 0;
326  }else{
327  return CF.mDistance;
328  }
329 }
330 
331 bool ColdetModel::checkCollisionWithPointCloud(const std::vector<Vector3> &i_cloud, double i_radius)
332 {
333  Opcode::SphereCollider SC;
334  SC.SetFirstContact(true);
335  Opcode::SphereCache Cache;
336  IceMaths::Point p(0,0,0);
337  IceMaths::Sphere sphere(p, i_radius);
338  IceMaths::Matrix4x4 sphereTrans(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1);
339  for (unsigned int i=0; i<i_cloud.size(); i++){
340  const Vector3& p = i_cloud[i];
341  sphereTrans.m[3][0] = p[0];
342  sphereTrans.m[3][1] = p[1];
343  sphereTrans.m[3][2] = p[2];
344  bool isOk = SC.Collide(Cache, sphere, dataSet->model, &sphereTrans, transform);
345  if (!isOk) std::cerr << "SphereCollider::Collide() failed" << std::endl;
346  if (SC.GetContactStatus()) return true;
347  }
348  return false;
349 }
350 
351 void ColdetModel::setNeighborTriangle(int triangle, int vertex0, int vertex1, int vertex2){
352  setNeighborTriangleSub(triangle, vertex0, vertex1);
353  setNeighborTriangleSub(triangle, vertex1, vertex2);
354  setNeighborTriangleSub(triangle, vertex2, vertex0);
355 }
356 
357 bool ColdetModel::setNeighborTriangleSub(int triangle, int vertex0, int vertex1){
358  Edge edge(vertex0, vertex1);
359  EdgeToTriangleMap::iterator p = triangleMap.find(edge);
360  if(p == triangleMap.end()){
361  triangleMap[edge].t[0] = triangle;
362  triangleMap[edge].t[1] = -1;
363  return true;
364  } else {
365  trianglePair& triangles = p->second;
366  if( triangles.t[1] != -1 ){
367  dataSet->neighbor[triangles.t[0]].deleteNeighbor(triangles.t[1]);
368  dataSet->neighbor[triangles.t[1]].deleteNeighbor(triangles.t[0]);
369  //cout << "neighbors[" << triangles.t[0] << "] " << neighbors[triangles.t[0]][0] << " " << neighbors[triangles.t[0]][1] << " " << neighbors[triangles.t[0]][2] << endl;
370  //cout << "neighbors[" << triangles.t[1] << "] " << neighbors[triangles.t[1]][0] << " " << neighbors[triangles.t[1]][1] << " " << neighbors[triangles.t[1]][2] << endl;
371  return false;
372  }else{
373  dataSet->neighbor[triangle].addNeighbor(triangles.t[0]);
374  dataSet->neighbor[triangles.t[0]].addNeighbor(triangle);
375  triangles.t[1] = triangle;
376  //cout << "neighbors[" << triangle << "] " << neighbors[triangle][0] << " " << neighbors[triangle][1] << " " << neighbors[triangle][2] << endl;
377  //cout << "neighbors[" << triangles.t[0] << "] " << neighbors[triangles.t[0]][0] << " " << neighbors[triangles.t[0]][1] << " " << neighbors[triangles.t[0]][2] << endl;
378  return true;
379  }
380  }
381 }
382 
384  dataSet->neighbor.resize(n);
385 }
386 
387 void ColdetModel::setNeighbor(int triangle0, int triangle1 ){
388 
389 }
390 
hrp::ColdetModel::setVertex
void setVertex(int index, float x, float y, float z)
add a vertex
Definition: ColdetModel.cpp:98
hrp::ColdetModel::getNumVertices
int getNumVertices() const
get the number of vertices
Definition: ColdetModel.cpp:79
hrp::ColdetModel::checkCollisionWithPointCloud
bool checkCollisionWithPointCloud(const std::vector< Vector3 > &i_cloud, double i_radius)
check collision between this triangle mesh and a point cloud
Definition: ColdetModel.cpp:331
i
png_uint_32 i
Definition: png.h:2732
hrp::ColdetModel
Definition: ColdetModel.h:61
Opcode.h
hrp::ColdetModelSharedDataSet::pType
ColdetModel::PrimitiveType pType
Definition: ColdetModelSharedDataSet.h:73
hrp::ColdetModelSharedDataSet::model
Opcode::Model model
Definition: ColdetModelSharedDataSet.h:66
hrp::ColdetModelSharedDataSet::numBBMap
std::vector< int > numBBMap
Definition: ColdetModelSharedDataSet.h:94
hrp::ColdetModelSharedDataSet::vertices
vector< IceMaths::Point > vertices
Definition: ColdetModelSharedDataSet.h:70
udword
unsigned int udword
sizeof(udword) must be 4
Definition: IceTypes.h:65
autoplay.n
n
Definition: autoplay.py:12
hrp::ColdetModel::triangleMap
EdgeToTriangleMap triangleMap
Definition: ColdetModel.h:260
hrp
Definition: ColdetModel.h:28
swingTest.f
f
Definition: swingTest.py:6
hrp::ColdetModel::build
void build()
build tree of bounding boxes to accelerate collision check
Definition: ColdetModel.cpp:143
hrp::ColdetModelSharedDataSet::computeDepth
int computeDepth(const Opcode::AABBCollisionNode *node, int currentDepth, int max)
Definition: ColdetModel.cpp:230
hrp::ColdetModel::setPosition
void setPosition(const Matrix33 &R, const Vector3 &p)
set position and orientation of this model
Definition: ColdetModel.cpp:258
hrp::ColdetModelSharedDataSet::refCounter
int refCounter
Definition: ColdetModelSharedDataSet.h:89
hrp::ColdetModel::~ColdetModel
virtual ~ColdetModel()
destructor
Definition: ColdetModel.cpp:63
hrp::ColdetModel::getAABBTreeDepth
int getAABBTreeDepth()
Definition: ColdetModel.cpp:197
hrp::ColdetModelSharedDataSet::AABBTreeMaxDepth
int AABBTreeMaxDepth
Definition: ColdetModelSharedDataSet.h:93
hrp::max
static int max(int a, int b)
Definition: MatrixSolvers.cpp:54
hrp::trianglePair::t
int t[2]
Definition: ColdetModel.h:56
hrp::ColdetModelSharedDataSet::numLeafMap
std::vector< int > numLeafMap
Definition: ColdetModelSharedDataSet.h:95
hrp::Vector3
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
hrp::ColdetModel::initialize
void initialize()
common part of constuctors
Definition: ColdetModel.cpp:43
hrp::Edge
Definition: ColdetModel.h:31
hrp::ColdetModel::numofBBtoDepth
int numofBBtoDepth(int minNumofBB)
Definition: ColdetModel.cpp:190
hrp::ColdetModelSharedDataSet::iMesh
Opcode::MeshInterface iMesh
Definition: ColdetModelSharedDataSet.h:68
value
png_voidp int value
Definition: png.h:2110
hrp::ColdetModel::getAABBmaxNum
int getAABBmaxNum()
Definition: ColdetModel.cpp:201
hrp::ColdetModel::getPrimitiveParam
bool getPrimitiveParam(unsigned int index, float &value) const
get a parameter of primitive
Definition: ColdetModel.cpp:298
hrp::ColdetModel::addTriangle
void addTriangle(int v1, int v2, int v3)
Definition: ColdetModel.cpp:137
hrp::ColdetModel::getPrimitiveType
PrimitiveType getPrimitiveType() const
get primitive type
Definition: ColdetModel.cpp:280
hrp::ColdetModel::addVertex
void addVertex(float x, float y, float z)
Definition: ColdetModel.cpp:104
hrp::ColdetModel::SP_MESH
@ SP_MESH
Definition: ColdetModel.h:64
hrp::ColdetModel::getTriangle
void getTriangle(int index, int &out_v1, int &out_v2, int &out_v3) const
Definition: ColdetModel.cpp:128
hrp::ColdetModelSharedDataSet
Definition: ColdetModelSharedDataSet.h:29
hrp::ColdetModelSharedDataSet::ColdetModelSharedDataSet
ColdetModelSharedDataSet()
Definition: ColdetModel.cpp:55
hrp::ColdetModel::setTriangle
void setTriangle(int index, int v1, int v2, int v3)
add a triangle
Definition: ColdetModel.cpp:119
hrp::ColdetModel::transform
IceMaths::Matrix4x4 * transform
Definition: ColdetModel.h:256
hrp::ColdetModelSharedDataSet::triangles
vector< IceMaths::IndexedTriangle > triangles
Definition: ColdetModelSharedDataSet.h:71
ColdetModel.h
hrp::ColdetModel::setPrimitivePosition
void setPrimitivePosition(const double *R, const double *p)
set position and orientation of primitive
Definition: ColdetModel.cpp:306
Ray
Definition: IceRay.h:15
hrp::ColdetModel::dataSet
ColdetModelSharedDataSet * dataSet
Definition: ColdetModel.h:255
hrp::ColdetModelSharedDataSet::neighbor
NeighborTriangleSetArray neighbor
Definition: ColdetModelSharedDataSet.h:76
hrp::ColdetModelSharedDataSet::getAABBTreeDepth
int getAABBTreeDepth()
Definition: ColdetModelSharedDataSet.h:78
hrp::ColdetModel::setPrimitiveType
void setPrimitiveType(PrimitiveType ptype)
set primitive type
Definition: ColdetModel.cpp:275
hrp::ColdetModelSharedDataSet::pParams
std::vector< float > pParams
Definition: ColdetModelSharedDataSet.h:74
hrp::trianglePair
Definition: ColdetModel.h:55
hrp::ColdetModel::setNeighborTriangleSub
bool setNeighborTriangleSub(int triangle, int vertex0, int vertex1)
Definition: ColdetModel.cpp:357
hrp::ColdetModel::setNumPrimitiveParams
void setNumPrimitiveParams(unsigned int nparam)
set the number of parameters of primitive
Definition: ColdetModel.cpp:285
hrp::ColdetModel::PrimitiveType
PrimitiveType
Definition: ColdetModel.h:64
getBoundingBoxDataSub
static void getBoundingBoxDataSub(const Opcode::AABBCollisionNode *node, unsigned int currentDepth, unsigned int depth, std::vector< Vector3 > &out_data)
Definition: ColdetModel.cpp:207
hrp::ColdetModel::initNeighbor
void initNeighbor(int n)
Definition: ColdetModel.cpp:383
hrp::ColdetModel::getNumTriangles
int getNumTriangles() const
Definition: ColdetModel.cpp:92
hrp::ColdetModelSharedDataSet::build
bool build()
Definition: ColdetModel.cpp:159
hrp::ColdetModel::setNeighbor
void setNeighbor(int triangle0, int triangle1)
Definition: ColdetModel.cpp:387
hrp::Matrix33
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
ColdetModelSharedDataSet.h
hrp::ColdetModel::isValid_
bool isValid_
Definition: ColdetModel.h:259
hrp::ColdetModelSharedDataSet::getmaxNumofBB
int getmaxNumofBB()
Definition: ColdetModelSharedDataSet.h:84
hrp::ColdetModel::pTransform
IceMaths::Matrix4x4 * pTransform
transform of primitive
Definition: ColdetModel.h:257
hrp::ColdetModel::setNumVertices
void setNumVertices(int n)
set the number of vertices
Definition: ColdetModel.cpp:73
hrp::ColdetModel::setNeighborTriangle
void setNeighborTriangle(int triangle, int vertex0, int vertex1, int vertex2)
Definition: ColdetModel.cpp:351
hrp::ColdetModel::setNumTriangles
void setNumTriangles(int n)
set the number of triangles
Definition: ColdetModel.cpp:85
hrp::ColdetModel::getBoundingBoxData
void getBoundingBoxData(const int depth, std::vector< Vector3 > &out_boxes)
Definition: ColdetModel.cpp:223
hrp::ColdetModel::getVertex
void getVertex(int index, float &out_x, float &out_y, float &out_z) const
get a vertex
Definition: ColdetModel.cpp:110
hrp::ColdetModel::computeDistanceWithRay
double computeDistanceWithRay(const double *point, const double *dir)
compute distance between a point and this mesh along ray
Definition: ColdetModel.cpp:314
hrp::ColdetModel::setPrimitiveParam
bool setPrimitiveParam(unsigned int index, float value)
set a parameter of primitive
Definition: ColdetModel.cpp:290


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:02