ColdetBody.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  * General Robotix Inc. 
00009  */
00014 #include "ColdetBody.h"
00015 #include <iostream>
00016 
00017 
00018 ColdetBody::ColdetBody(BodyInfo_ptr bodyInfo)
00019 {
00020     LinkInfoSequence_var links = bodyInfo->links();
00021     ShapeInfoSequence_var shapes = bodyInfo->shapes();
00022 
00023     int numLinks = links->length();
00024 
00025     linkColdetModels.resize(numLinks);
00026                 
00027     for(int linkIndex = 0; linkIndex < numLinks ; ++linkIndex){
00028 
00029         LinkInfo& linkInfo = links[linkIndex];
00030                         
00031         int totalNumTriangles = 0;
00032         int totalNumVertices = 0;
00033         const TransformedShapeIndexSequence& shapeIndices = linkInfo.shapeIndices;
00034         short shapeIndex;
00035         double R[9], p[3];
00036         unsigned int nshape = shapeIndices.length();
00037             for(unsigned int i=0; i < shapeIndices.length(); i++){
00038             shapeIndex = shapeIndices[i].shapeIndex;
00039             const DblArray12 &tform = shapeIndices[i].transformMatrix;
00040             R[0] = tform[0]; R[1] = tform[1]; R[2] = tform[2]; p[0] = tform[3];
00041             R[3] = tform[4]; R[4] = tform[5]; R[5] = tform[6]; p[1] = tform[7];
00042             R[6] = tform[8]; R[7] = tform[9]; R[8] = tform[10]; p[2] = tform[11];
00043             const ShapeInfo& shapeInfo = shapes[shapeIndex];
00044             totalNumTriangles += shapeInfo.triangles.length() / 3;
00045             totalNumVertices += shapeInfo.vertices.length() / 3;
00046         }
00047 
00048         const SensorInfoSequence& sensors = linkInfo.sensors;
00049         for (unsigned int i=0; i<sensors.length(); i++){
00050             const SensorInfo &sinfo = sensors[i];
00051             const TransformedShapeIndexSequence tsis = sinfo.shapeIndices;
00052             nshape += tsis.length();
00053             for (unsigned int j=0; j<tsis.length(); j++){
00054                 shapeIndex = tsis[j].shapeIndex;
00055                 const DblArray12 &tform = tsis[j].transformMatrix;
00056                 R[0] = tform[0]; R[1] = tform[1]; R[2] = tform[2]; p[0] = tform[3];
00057                 R[3] = tform[4]; R[4] = tform[5]; R[5] = tform[6]; p[1] = tform[7];
00058                 R[6] = tform[8]; R[7] = tform[9]; R[8] = tform[10]; p[2] = tform[11];
00059                 const ShapeInfo& shapeInfo = shapes[shapeIndex];
00060                 totalNumTriangles += shapeInfo.triangles.length() / 3;
00061                 totalNumVertices += shapeInfo.vertices.length() / 3 ;
00062             }
00063         }
00064 
00065         //int totalNumVertices = totalNumTriangles * 3;
00066 
00067         ColdetModelPtr coldetModel(new ColdetModel());
00068         coldetModel->setName(std::string(linkInfo.name));
00069         
00070         if(totalNumTriangles > 0){
00071             coldetModel->setNumVertices(totalNumVertices);
00072             coldetModel->setNumTriangles(totalNumTriangles);
00073             if (nshape == 1){
00074                 addLinkPrimitiveInfo(coldetModel, R, p, shapes[shapeIndex]);
00075             }
00076             addLinkVerticesAndTriangles(coldetModel, linkInfo, shapes);
00077             coldetModel->build();
00078         }
00079 
00080         linkColdetModels[linkIndex] = coldetModel;
00081         linkNameToColdetModelMap.insert(make_pair(std::string (linkInfo.name),
00082                                                   coldetModel));
00083 
00084         cout << linkInfo.name << " has "<< totalNumTriangles << " triangles." << endl;
00085     }
00086 }
00087 
00088 void ColdetBody::addLinkPrimitiveInfo(ColdetModelPtr& coldetModel, 
00089                                       const double *R, const double *p,
00090                                       const ShapeInfo& shapeInfo)
00091 {
00092     switch(shapeInfo.primitiveType){
00093     case SP_BOX:
00094         coldetModel->setPrimitiveType(ColdetModel::SP_BOX);
00095         break;
00096     case SP_CYLINDER:
00097         coldetModel->setPrimitiveType(ColdetModel::SP_CYLINDER);
00098         break;
00099     case SP_CONE:
00100         coldetModel->setPrimitiveType(ColdetModel::SP_CONE);
00101         break;
00102     case SP_SPHERE:
00103         coldetModel->setPrimitiveType(ColdetModel::SP_SPHERE);
00104         break;
00105     case SP_PLANE:
00106         coldetModel->setPrimitiveType(ColdetModel::SP_PLANE);
00107         break;
00108     default:
00109         break;
00110     }
00111     coldetModel->setNumPrimitiveParams(shapeInfo.primitiveParameters.length());
00112     for (unsigned int i=0; i<shapeInfo.primitiveParameters.length(); i++){
00113         coldetModel->setPrimitiveParam(i, shapeInfo.primitiveParameters[i]);
00114     }
00115     coldetModel->setPrimitivePosition(R, p);
00116 }
00117 
00118 void ColdetBody::addLinkVerticesAndTriangles
00119 (ColdetModelPtr& coldetModel, const TransformedShapeIndex& tsi, const Matrix44& Tparent, ShapeInfoSequence_var& shapes, int& vertexIndex, int& triangleIndex)
00120 {
00121     short shapeIndex = tsi.shapeIndex;
00122     const DblArray12& M = tsi.transformMatrix;;
00123     Matrix44 T, Tlocal;
00124     Tlocal << M[0], M[1], M[2],  M[3],
00125              M[4], M[5], M[6],  M[7],
00126              M[8], M[9], M[10], M[11],
00127              0.0,  0.0,  0.0,   1.0;
00128     T = Tparent * Tlocal;
00129     
00130     const ShapeInfo& shapeInfo = shapes[shapeIndex];
00131     int vertexIndexBase = vertexIndex;
00132     const FloatSequence& vertices = shapeInfo.vertices;
00133     const int numVertices = vertices.length() / 3;
00134     for(int j=0; j < numVertices; ++j){
00135         Vector4 v(T * Vector4(vertices[j*3], vertices[j*3+1], vertices[j*3+2], 1.0));
00136         coldetModel->setVertex(vertexIndex++, v[0], v[1], v[2]);
00137     }
00138 
00139     const LongSequence& triangles = shapeInfo.triangles;
00140     const int numTriangles = triangles.length() / 3;
00141     for(int j=0; j < numTriangles; ++j){
00142        int t0 = triangles[j*3] + vertexIndexBase;
00143        int t1 = triangles[j*3+1] + vertexIndexBase;
00144        int t2 = triangles[j*3+2] + vertexIndexBase;
00145        coldetModel->setTriangle( triangleIndex++, t0, t1, t2);
00146     }
00147     
00148 }
00149 
00150 void ColdetBody::addLinkVerticesAndTriangles
00151 (ColdetModelPtr& coldetModel, LinkInfo& linkInfo, ShapeInfoSequence_var& shapes)
00152 {
00153     int vertexIndex = 0;
00154     int triangleIndex = 0;
00155 
00156     const TransformedShapeIndexSequence& shapeIndices = linkInfo.shapeIndices;
00157 
00158     Matrix44 E(Matrix44::Identity());
00159     for(unsigned int i=0; i < shapeIndices.length(); i++){
00160         addLinkVerticesAndTriangles(coldetModel, shapeIndices[i], E, shapes,
00161                                     vertexIndex, triangleIndex);
00162     }
00163 
00164     Matrix44 T(Matrix44::Identity());
00165     const SensorInfoSequence& sensors = linkInfo.sensors;
00166     for (unsigned int i=0; i<sensors.length(); i++){
00167         const SensorInfo& sensor = sensors[i]; 
00168         calcRodrigues(T, Vector3(sensor.rotation[0], sensor.rotation[1], 
00169                                  sensor.rotation[2]), sensor.rotation[3]);
00170         T(0,3) = sensor.translation[0];
00171         T(1,3) = sensor.translation[1];
00172         T(2,3) = sensor.translation[2];
00173         const TransformedShapeIndexSequence& shapeIndices = sensor.shapeIndices;
00174         for (unsigned int j=0; j<shapeIndices.length(); j++){
00175             addLinkVerticesAndTriangles(coldetModel, shapeIndices[j], T, 
00176                                         shapes,
00177                                         vertexIndex, triangleIndex);
00178         }
00179     }
00180 }
00181 
00182 
00183 ColdetBody::ColdetBody(const ColdetBody& org)
00184 {
00185     linkColdetModels = org.linkColdetModels;
00186     linkNameToColdetModelMap = org.linkNameToColdetModelMap;
00187 }
00188 
00189 
00190 void ColdetBody::setLinkPositions(const LinkPositionSequence& linkPositions)
00191 {
00192     const int srcNumLinks = linkPositions.length();
00193     const int selfNumLinks = linkColdetModels.size();
00194     for(int i=0; i < srcNumLinks && i < selfNumLinks; ++i){
00195         const LinkPosition& linkPosition = linkPositions[i];
00196         if(linkColdetModels[i]){
00197             linkColdetModels[i]->setPosition(linkPosition.R, linkPosition.p);
00198         }
00199     }
00200 }


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