00001 #include "SIGEL_Robot/SIG_Body.h"
00002 #include "SIGEL_Robot/SIG_Polygon.h"
00003 #include <cmath>
00004 #include "SIGEL_Robot/SIG_GeometryIterator.h"
00005
00006 namespace SIGEL_Robot
00007 {
00008 SIG_Body::SIG_Body(SIG_Robot *par, QString f, QString d)
00009 : parent(par),
00010 geometryFile(f),
00011 directory (d),
00012 geometry(0)
00013 { };
00014
00015 SIG_Body::SIG_Body(SIG_Robot *par, QTextStream & tx)
00016 : parent (par)
00017 {
00018 QString tmpstr;
00019
00020 tx >> geometryFile;
00021 tx >> directory;
00022 tx >> tmpstr;
00023 if (tmpstr == "y") {
00024 geometry = new SIG_Geometry (tx);
00025 } else {
00026 geometry = 0;
00027 }
00028 };
00029
00030 SIG_Body::~SIG_Body(void)
00031 {
00032 usedByLinks.setAutoDelete (FALSE);
00033 delete geometry;
00034 };
00035
00036 void SIG_Body::addUsingLink (SIG_Link *user)
00037 {
00038 usedByLinks.append (user);
00039 };
00040
00041 NEWMAT::Matrix SIG_Body::createRotationMatrix( QArray< float > rotation )
00042 {
00043 NEWMAT::Matrix rotationMatrix(4,4);
00044 double const x = rotation[0];
00045 double const y = rotation[1];
00046 double const z = rotation[2];
00047 double const a = rotation[3];
00048
00049 double const sinA = std::sin(a);
00050 double const cosA = std::cos(a);
00051 double const t = 1 - cosA;
00052
00053 rotationMatrix = 0;
00054 rotationMatrix(1,1) = (t * x * x) + cosA;
00055 rotationMatrix(1,2) = (t * x * y) + (sinA * z);
00056 rotationMatrix(1,3) = (t * x * z) - (sinA * y);
00057 rotationMatrix(2,1) = (t * x * y) - (sinA * z);
00058 rotationMatrix(2,2) = (t * y * y) + cosA;
00059 rotationMatrix(2,3) = (t * y * z) + (sinA * x);
00060 rotationMatrix(3,1) = (t * x * z) + (sinA * y);
00061 rotationMatrix(3,2) = (t * y * z) - (sinA * x);
00062 rotationMatrix(3,3) = (t * z * z) + cosA;
00063 rotationMatrix(4,4) = 1;
00064
00065 return rotationMatrix;
00066 };
00067
00068 void SIG_Body::readVRMLNode(Node *node, NEWMAT::Matrix transformation)
00069 {
00070 if ( node->isGroupNode() || node->isTransformNode() ) {
00071 if (node->isTransformNode()) {
00072 TransformNode *transformNode = static_cast<TransformNode*> (node);
00073
00074 QArray< float > buffer(4);
00075
00076 NEWMAT::Matrix identity(4,4);
00077 identity = 0;
00078 for (int i=1; i<=4; i++)
00079 identity(i,i) = 1;
00080
00081 NEWMAT::Matrix center(4,4);
00082 center = identity;
00083 transformNode->getCenter( buffer.data() );
00084 for (int i=0; i<3; i++)
00085 center( i+1, 4 ) = buffer[i];
00086
00087 NEWMAT::Matrix rotation(4,4);
00088 transformNode->getRotation( buffer.data() );
00089 rotation = createRotationMatrix( buffer );
00090
00091 NEWMAT::Matrix scale(4,4);
00092 scale = 0;
00093 scale(4,4) = 1;
00094 transformNode->getScale( buffer.data() );
00095 for (int i=0; i<3; i++)
00096 scale( i+1, i+1 ) = buffer[i];
00097
00098 NEWMAT::Matrix scaleOrientation(4,4);
00099 transformNode->getScaleOrientation( buffer.data() );
00100 scaleOrientation = createRotationMatrix( buffer );
00101
00102 NEWMAT::Matrix translation(4,4);
00103 translation = identity;
00104 transformNode->getTranslation( buffer.data() );
00105 for (int i=0; i<3; i++)
00106 translation( i+1, 4 ) = buffer[i];
00107
00108 transformation = transformation
00109 * translation
00110 * center
00111 * rotation
00112 * scaleOrientation
00113 * scale
00114 * scaleOrientation.i()
00115 * center.i();
00116 };
00117
00118 for (Node *child = node->getChildNodes(); child; child = child->next())
00119 readVRMLNode(child, transformation);
00120 } else if ( node->isShapeNode() ) {
00121 ShapeNode *shapeNode = static_cast<ShapeNode*> (node);
00122
00123 GeometryNode *geometryNode = shapeNode->getGeometry();
00124
00125 if (geometryNode)
00126 if (geometryNode->isIndexedFaceSetNode()) {
00127 IndexedFaceSetNode *indexedFaceSetNode
00128 = static_cast<IndexedFaceSetNode*> (geometryNode);
00129
00130 CoordinateNode *coordinateNode = indexedFaceSetNode->getCoordinateNodes();
00131 if (coordinateNode) {
00132 int noOfVertices = coordinateNode->getNPoints();
00133 int noOfIndices = indexedFaceSetNode->getNCoordIndexes();
00134 QVector< DL_vector > vertices( noOfVertices );
00135 vertices.setAutoDelete( true );
00136
00137 for (int i=0; i < noOfVertices; i++) {
00138 QArray< float > coords(3);
00139
00140 coordinateNode->getPoint( i, coords.data() );
00141
00142 NEWMAT::ColumnVector actVertex(4);
00143 for (int j=0; j<3; j++)
00144 actVertex( j+1 ) = coords[j];
00145 actVertex(4) = 1;
00146
00147 actVertex = transformation * actVertex;
00148
00149 DL_vector *finalVertex = new DL_vector();
00150
00151 for (int j = 0; j<3; j++)
00152 finalVertex->set( j, actVertex( j+1 ) );
00153
00154 vertices.insert( i, finalVertex );
00155 };
00156
00157 SIG_Polygon *actPolygon = 0;
00158
00159 for (int i=0; i < noOfIndices; i++) {
00160 int actIndex = indexedFaceSetNode->getCoordIndex(i);
00161
00162 if (actIndex < 0)
00163 actPolygon = 0;
00164 else {
00165 if (!actPolygon)
00166 actPolygon = new SIG_Polygon( geometry );
00167
00168 actPolygon->appendVertex( *vertices[ actIndex ] );
00169 };
00170 };
00171 };
00172 };
00173 };
00174 };
00175
00176 void SIG_Body::load()
00177 {
00178
00179
00180 if (geometry) return;
00181
00182 geometry = new SIG_Geometry ();
00183
00184 SceneGraph *bodyScene = new SceneGraph();
00185
00186
00187 bodyScene->load( const_cast<char*>( getGeometryFile ().latin1() ) );
00188
00189 NEWMAT::Matrix identity(4,4);
00190 identity = 0;
00191 for (int i=1; i<=4; i++)
00192 identity(i,i) = 1;
00193
00194 for (Node *actNode = bodyScene->getNodes(); actNode; actNode = actNode->next())
00195 readVRMLNode( actNode, identity);
00196 }
00197
00198 void SIG_Body::setGeometry (SIG_Geometry *geo)
00199 {
00200 geometry = geo;
00201 };
00202
00203 SIG_Geometry const *SIG_Body::getGeometry (void) const
00204 {
00205 return geometry;
00206 };
00207
00208 QString SIG_Body::getGeometryFile (void) const
00209 {
00210 if (geometryFile.at (0) == '/')
00211 return geometryFile;
00212 else {
00213 QString retval;
00214 retval = directory;
00215 retval.append (geometryFile);
00216 return retval;
00217 }
00218 }
00219
00220 QString SIG_Body::getName (void) const
00221 {
00222 return geometryFile;
00223 }
00224
00225 void SIG_Body::writeToFileTransfer (QTextStream & tx)
00226 {
00227 tx << "Body " << geometryFile << ' ' << directory << ' ';
00228 if (geometry) {
00229 tx << "y\n";
00230 geometry->writeToFileTransfer (tx);
00231 } else
00232 tx << "n\n";
00233 };
00234 }
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266