Main Page   Namespace List   Class Hierarchy   Compound List   File List   Namespace Members   Compound Members  

SIG_Body.cpp

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                 // "Body" wurde bereits in SIG_Robot gelesen.
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                 // If there is already a geometry, don't create another!
00179                 // To Do: Replace the simple return with an exception.
00180                 if (geometry) return;
00181                 
00182                 geometry = new SIG_Geometry ();
00183                 
00184                 SceneGraph *bodyScene = new SceneGraph();
00185                 // Load expects a char*, so a const_cast is necessary.
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 

Generated at Mon Sep 3 01:32:18 2001 for PG 368 - SIGEL by doxygen1.2.3 written by Dimitri van Heesch, © 1997-2000