00001
00002
00003
00004
00005
00006
00007
00008
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
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 }