00001
00002
00003
00004
00005
00006
00007
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
00148
00149
00150
00151
00152
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
00234
00235
00236
00237
00238
00239
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
00370
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
00377
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