Robotsystem from Robot Group Leipzig: primitive.cpp Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# primitive.cpp

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius@informatik.uni-leipzig.de                                    *
    00004  *    fhesse@informatik.uni-leipzig.de                                     *
    00005  *    der@informatik.uni-leipzig.de                                        *
    00006  *                                                                         *
    00007  *   This program is free software; you can redistribute it and/or modify  *
    00008  *   it under the terms of the GNU General Public License as published by  *
    00009  *   the Free Software Foundation; either version 2 of the License, or     *
    00010  *   (at your option) any later version.                                   *
    00011  *                                                                         *
    00012  *   This program is distributed in the hope that it will be useful,       *
    00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00015  *   GNU General Public License for more details.                          *
    00016  *                                                                         *
    00017  *   You should have received a copy of the GNU General Public License     *
    00018  *   along with this program; if not, write to the                         *
    00019  *   Free Software Foundation, Inc.,                                       *
    00020  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00021  *                                                                         *
    00022  ***************************************************************************
    00023  *                                                                         *
    00024  *   $Log: primitive.cpp,v $
    00025  *   Revision 1.1.2.12  2006/03/30 12:34:53  martius
    00026  *   documentation updated
    00027  *
    00028  *   Revision 1.1.2.11  2006/03/29 15:06:58  martius
    00029  *   update on setPose
    00030  *
    00031  *   Revision 1.1.2.10  2006/01/31 15:45:28  martius
    00032  *   proper destruction
    00033  *
    00034  *   Revision 1.1.2.9  2006/01/27 13:06:21  fhesse
    00035  *   getPose/getPosition return pose/Position of geom if no body exists
    00036  *
    00037  *   Revision 1.1.2.8  2006/01/12 14:21:00  martius
    00038  *   drawmode, material
    00039  *
    00040  *   Revision 1.1.2.7  2006/01/03 10:06:16  fhesse
    00041  *   primitive::getGeom and getBody return 0 if it is zero pointer
    00042  *
    00043  *   Revision 1.1.2.6  2005/12/15 17:03:43  martius
    00044  *   cameramanupulator setPose is working
    00045  *   joints have setter and getter parameters
    00046  *   Primitives are not longer inherited from OSGPrimitive, moreover
    00047  *   they aggregate them
    00048  *
    00049  *   Revision 1.1.2.5  2005/12/14 15:36:45  martius
    00050  *   joints are visible now
    00051  *
    00052  *   Revision 1.1.2.4  2005/12/13 18:11:13  martius
    00053  *   transform primitive added, some joints stuff done, forward declaration
    00054  *
    00055  *   Revision 1.1.2.3  2005/12/11 23:35:08  martius
    00056  *   *** empty log message ***
    00057  *
    00058  *   Revision 1.1.2.2  2005/12/09 16:54:16  martius
    00059  *   camera is woring now
    00060  *
    00061  *   Revision 1.1.2.1  2005/12/06 10:13:25  martius
    00062  *   openscenegraph integration started
    00063  *
    00064  *                                                                 *
    00065  *                                                                         *
    00066  ***************************************************************************/
    00067
    00068 #include <assert.h>
    00069 #include <osg/MatrixTransform>
    00070
    00071 #include "primitive.h"
    00072 #include "pos.h"
    00073
    00074 namespace lpzrobots{
    00075
    00076   // returns the osg (4x4) pose matrix of the ode geom
    00077   osg::Matrix osgPose( dGeomID geom ){
    00078     return osgPose(dGeomGetPosition(geom), dGeomGetRotation(geom));
    00079   }
    00080
    00081   // returns the osg (4x4) pose matrix of the ode body
    00082   osg::Matrix osgPose( dBodyID body ){
    00083     return osgPose(dBodyGetPosition(body), dBodyGetRotation(body));
    00084   }
    00085
    00086   // converts a position vector and a rotation matrix from ode to osg 4x4 matrix
    00087   osg::Matrix osgPose( const double * V , const double * R ){
    00088     return osg::Matrix( R[0], R[4], R[8],  0,
    00089                         R[1], R[5], R[9],  0,
    00090                         R[2], R[6], R[10], 0,
    00091                         V[0], V[1], V[2] , 1);
    00092   }
    00093
    00094   // converts a position vector and a rotation matrix from ode to osg 4x4 matrix
    00095   void odeRotation( const osg::Matrix& pose , dMatrix3& odematrix){
    00096     osg::Quat q;
    00097     pose.get(q);
    00098     // THIS should be
    00099     //    dQuaternion quat = {q.x(), q.y(), q.z(), q.w() };
    00100     dQuaternion quat = {q.w(), q.x(), q.y(), q.z() };
    00101     dQtoR(quat, odematrix);
    00102   }
    00103
    00104
    00105
    00106   /******************************************************************************/
    00107
    00108   Primitive::Primitive()
    00109     : geom(0), body(0) {
    00110   }
    00111
    00112   Primitive::~Primitive () {
    00113     if(geom) dGeomDestroy( geom );
    00114     if(body) dBodyDestroy( body );
    00115   }
    00116
    00117
    00118   void Primitive::setPosition(const osg::Vec3& pos){
    00119     if(body){
    00120       dBodySetPosition(body, pos.x(), pos.y(), pos.z());
    00121     }else if(geom){ // okay there is just a geom no body
    00122       dGeomSetPosition(geom, pos.x(), pos.y(), pos.z());
    00123     }
    00124     update(); // update the scenegraph stuff
    00125   }
    00126
    00127   void Primitive::setPose(const osg::Matrix& pose){
    00128     if(body){
    00129       osg::Vec3 pos = pose.getTrans();
    00130       dBodySetPosition(body, pos.x(), pos.y(), pos.z());
    00131       osg::Quat q;
    00132       pose.get(q);
    00133       // this should be
    00134       //      dReal quat[4] = {q.x(), q.y(), q.z(), q.w()};
    00135       dReal quat[4] = {q.w(), q.x(), q.y(), q.z()};
    00136       dBodySetQuaternion(body, quat);
    00137     }else if(geom){ // okay there is just a geom no body
    00138       osg::Vec3 pos = pose.getTrans();
    00139       dGeomSetPosition(geom, pos.x(), pos.y(), pos.z());
    00140       osg::Quat q;
    00141       pose.get(q);
    00142       // this should be
    00143       // dReal quat[4] = {q.x(), q.y(), q.z(), q.w()};
    00144       dReal quat[4] = {q.w(), q.x(), q.y(), q.z()};
    00145       dGeomSetQuaternion(geom, quat);
    00146     }
    00147     update(); // update the scenegraph stuff
    00148   }
    00149
    00150   osg::Vec3 Primitive::getPosition() const {
    00151     if(!body) return Pos(dGeomGetPosition(geom));
    00152     return Pos(dBodyGetPosition(body));
    00153   }
    00154
    00155   osg::Matrix Primitive::getPose() const {
    00156     if(!body) return osgPose(dGeomGetPosition(geom), dGeomGetRotation(geom));
    00157     return osgPose(dBodyGetPosition(body), dBodyGetRotation(body));
    00158   }
    00159
    00160   dGeomID Primitive::getGeom() const {
    00161    if (this) return geom;
    00162    else return 0;
    00163   }
    00164
    00165   dBodyID Primitive::getBody() const {
    00166    if (this) return body;
    00167    else return 0;
    00168   }
    00169
    00170   /******************************************************************************/
    00171   Plane::Plane(){
    00172     osgplane = new OSGPlane();
    00173   }
    00174
    00175   Plane::~Plane(){
    00176     if(osgplane) delete osgplane;
    00177   }
    00178
    00179   void Plane::init(const OdeHandle& odeHandle, double mass, const OsgHandle& osgHandle,
    00180                    char mode) {
    00181     assert(mode & Body || mode & Geom);
    00182     this->mode=mode;
    00183     if (mode & Body){
    00184       body = dBodyCreate (odeHandle.world);
    00185       dMass m;
    00186       dMassSetBox(&m,1,1000,1000,0.01); // fake the mass of the plane with a thin box
    00187       dMassAdjust (&m, mass);
    00188       dBodySetMass (body,&m); //assign the mass to the body
    00189     }
    00190     if(mode & Geom){
    00191       geom = dCreatePlane ( odeHandle.space , 0 , 0 , 1 , 0 );
    00192       if(mode & Body)
    00193         dGeomSetBody (geom, body); // geom is assigned to body
    00194     }
    00195     if(mode & Draw){
    00196       osgplane->init(osgHandle);
    00197     }
    00198
    00199   }
    00200
    00201   void Plane:: update(){
    00202     if(mode & Draw) {
    00203       if(body)
    00204         osgplane->setMatrix(osgPose(body));
    00205       else
    00206         osgplane->setMatrix(osgPose(geom));
    00207     }
    00208   }
    00209
    00210   /******************************************************************************/
    00211   Box::Box(float lengthX, float lengthY, float lengthZ) {
    00212     osgbox = new OSGBox(lengthX, lengthY, lengthZ);
    00213   }
    00214
    00215   Box::~Box(){
    00216     if(osgbox) delete osgbox;
    00217   }
    00218
    00219   void Box::init(const OdeHandle& odeHandle, double mass, const OsgHandle& osgHandle,
    00220                  char mode) {
    00221     assert((mode & Body) || (mode & Geom));
    00222     this->mode=mode;
    00223     if (mode & Body){
    00224       body = dBodyCreate (odeHandle.world);
    00225       dMass m;
    00226       // fake the mass of the plane with a thin box
    00227       dMassSetBox(&m, 1, osgbox->getLengthX() , osgbox->getLengthY() , osgbox->getLengthZ());
    00228       dMassAdjust (&m, mass);
    00229       dBodySetMass (body,&m); //assign the mass to the body
    00230     }
    00231     if (mode & Geom){
    00232       geom = dCreateBox ( odeHandle.space , osgbox->getLengthX() ,
    00233                           osgbox->getLengthY() , osgbox->getLengthZ());
    00234       if (mode & Body){
    00235         dGeomSetBody (geom, body); // geom is assigned to body
    00236       }
    00237     }
    00238     if (mode & Draw){
    00239       osgbox->init(osgHandle);
    00240     }
    00241   }
    00242
    00243   void Box:: update(){
    00244     if(mode & Draw) {
    00245       if(body)
    00246         osgbox->setMatrix(osgPose(body));
    00247       else
    00248         osgbox->setMatrix(osgPose(geom));
    00249     }
    00250   }
    00251
    00252   /******************************************************************************/
    00253   Sphere::Sphere(float radius) {
    00254     osgsphere = new OSGSphere(radius);
    00255   }
    00256
    00257   Sphere::~Sphere(){
    00258     if(osgsphere) delete osgsphere;
    00259   }
    00260
    00261   void Sphere::init(const OdeHandle& odeHandle, double mass, const OsgHandle& osgHandle,
    00262                     char mode) {
    00263     assert(mode & Body || mode & Geom);
    00264     this->mode=mode;
    00265     if (mode & Body){
    00266       body = dBodyCreate (odeHandle.world);
    00267       dMass m;
    00268       dMassSetSphere(&m, 1, osgsphere->getRadius());
    00269       dMassAdjust (&m, mass);
    00270       dBodySetMass (body,&m); //assign the mass to the body
    00271     }
    00272     if (mode & Geom){
    00273       geom = dCreateSphere ( odeHandle.space , osgsphere->getRadius());
    00274       if (mode & Body)
    00275         dGeomSetBody (geom, body); // geom is assigned to body
    00276     }
    00277     if (mode & Draw){
    00278       osgsphere->init(osgHandle);
    00279     }
    00280
    00281   }
    00282
    00283   void Sphere::update(){
    00284     if(mode & Draw) {
    00285       if(body)
    00286         osgsphere->setMatrix(osgPose(body));
    00287       else
    00288         osgsphere->setMatrix(osgPose(geom));
    00289     }
    00290   }
    00291
    00292   /******************************************************************************/
    00293   Capsule::Capsule(float radius, float height) {
    00294     osgcapsule = new OSGCapsule(radius, height);
    00295   }
    00296
    00297   Capsule::~Capsule(){
    00298     if(osgcapsule) delete osgcapsule;
    00299   }
    00300
    00301   void Capsule::init(const OdeHandle& odeHandle, double mass, const OsgHandle& osgHandle,
    00302                      char mode) {
    00303     assert(mode & Body || mode & Geom);
    00304     this->mode=mode;
    00305     if (mode & Body){
    00306       body = dBodyCreate (odeHandle.world);
    00307       dMass m;
    00308       dMassSetCappedCylinder(&m, 1.0, 3 , osgcapsule->getRadius(), osgcapsule->getHeight());
    00309       dMassAdjust (&m, mass);
    00310       dBodySetMass (body,&m); //assign the mass to the body
    00311     }
    00312     if (mode & Geom){
    00313       geom = dCreateCCylinder ( odeHandle.space , osgcapsule->getRadius(), osgcapsule->getHeight());
    00314       if (mode & Body)
    00315         dGeomSetBody (geom, body); // geom is assigned to body
    00316     }
    00317     if (mode & Draw){
    00318       osgcapsule->init(osgHandle);
    00319     }
    00320   }
    00321
    00322   void Capsule::update(){
    00323     if(mode & Draw) {
    00324       if(body)
    00325         osgcapsule->setMatrix(osgPose(body));
    00326       else
    00327         osgcapsule->setMatrix(osgPose(geom));
    00328     }
    00329   }
    00330
    00331   /******************************************************************************/
    00332   Transform::Transform(Primitive* parent, Primitive* child, const osg::Matrix& pose)
    00333     : parent(parent), child(child), pose(pose) {
    00334   }
    00335
    00336   void Transform::init(const OdeHandle& odeHandle, double mass, const OsgHandle& osgHandle,
    00337                        char mode) {
    00338     // we ignore the mode!
    00339     assert(parent && parent->getBody() != 0 && child); // parent and child must exist
    00340     assert(child->getBody() == 0 && child->getGeom() == 0); // child should not be initialised
    00341
    00342     // our own geom is just a transform
    00343     geom = dCreateGeomTransform(odeHandle.space);
    00344     dGeomTransformSetInfo(geom, 1);
    00345     dGeomTransformSetCleanup(geom, 1);
    00346
    00347     // the child geom must go into space 0 (because it inherits the space from the transform geom)
    00348     OdeHandle odeHandleChild(odeHandle);
    00349     odeHandleChild.space = 0;
    00350     // the root node for the child is the transform node of the parent
    00351     OsgHandle osgHandleChild(osgHandle);
    00352     osgHandleChild.scene = parent->getOSGPrimitive()->getTransform();
    00353     assert(osgHandleChild.scene);
    00354     // initialise the child
    00355     child->init(odeHandleChild, 0, osgHandleChild, Primitive::Geom | Primitive::Draw);
    00356     // move the child to the right place (in local coordinates)
    00357     child->setPose(pose);
    00358
    00359     // assoziate the child with the transform geom
    00360     dGeomTransformSetGeom (geom, child->getGeom());
    00361     // finally bind the transform the body of parent
    00362     dGeomSetBody (geom, parent->getBody());
    00363   }
    00364
    00365   void Transform::update(){
    00366     if(child) child->update();
    00367   }
    00368
    00369 }