00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00030 #include <cmath>
00031 #include <fstream>
00032 #include <iostream>
00033 #include <iomanip>
00034 #include <sstream>
00035 #include <vector>
00036 #include <QFile>
00037 #include <QString>
00038 #include <QTextStream>
00039
00040 #include "body.h"
00041 #include "collisionInterface.h"
00042 #include "bBox.h"
00043 #include "triangle.h"
00044 #include "world.h"
00045 #include "robot.h"
00046 #include "joint.h"
00047 #include "dynJoint.h"
00048 #include "ivmgr.h"
00049 #include "contact.h"
00050 #include "graspitGUI.h"
00051 #include "tinyxml.h"
00052
00053 #ifdef PLY_READER
00054 #include "mesh_loader.h"
00055 #endif
00056
00057 extern "C" {
00058 #include "maxdet.h"
00059 }
00060 #include "mytools.h"
00061
00062 #include <Inventor/SoDB.h>
00063 #include <Inventor/SoInput.h>
00064 #include <Inventor/actions/SoGetBoundingBoxAction.h>
00065 #include <Inventor/actions/SoSearchAction.h>
00066 #include <Inventor/nodes/SoSeparator.h>
00067 #include <Inventor/nodes/SoCoordinate3.h>
00068 #include <Inventor/nodes/SoCylinder.h>
00069 #include <Inventor/nodes/SoCube.h>
00070 #include <Inventor/nodes/SoIndexedFaceSet.h>
00071 #include <Inventor/nodes/SoMaterial.h>
00072 #include <Inventor/VRMLnodes/SoVRMLMaterial.h>
00073 #include <Inventor/VRMLnodes/SoVRMLAppearance.h>
00074 #include <Inventor/VRMLnodes/SoVRMLShape.h>
00075 #include <Inventor/nodes/SoPickStyle.h>
00076 #include <Inventor/nodes/SoSwitch.h>
00077 #include <Inventor/nodes/SoTransform.h>
00078 #include <Inventor/nodes/SoTranslation.h>
00079 #include <Inventor/draggers/SoRotateDiscDragger.h>
00080 #include <Inventor/nodekits/SoWrapperKit.h>
00081 #include <Inventor/SoPrimitiveVertex.h>
00082 #include <Inventor/actions/SoCallbackAction.h>
00083 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
00084
00085 #include <Inventor/actions/SoWriteAction.h>
00086
00087 #ifdef USE_DMALLOC
00088 #include "dmalloc.h"
00089 #endif
00090
00091
00092 #include "debug.h"
00093
00094 #define SQR(x) ((x)*(x))
00095 #define CUBE(x) ((x)*(x)*(x))
00096 #define AXES_SCALE 100.0
00097
00098 const float Body::CONE_HEIGHT = 20.0;
00099 double DynamicBody::defaultMass = 300.0;
00100
00106 Body::Body(World *w,const char *name) : WorldElement(w,name)
00107 {
00108
00109 mIsElastic = false;
00110 youngMod = -1;
00111
00112 material = -1;
00113 numContacts=0;
00114 showFC = false;
00115 showVC = false;
00116 IVGeomRoot = NULL; IVTran = NULL; IVMat = NULL; IVContactIndicators=NULL;
00117 IVBVRoot = NULL;
00118 #ifdef GEOMETRY_LIB
00119 IVPrimitiveRoot = NULL;
00120 #endif
00121 mUsesFlock = false;
00122 mBirdNumber = 0;
00123 mRenderGeometry = true;
00124 mGeometryFilename = "none";
00125
00126 initializeIV();
00127 }
00128
00132 Body::Body(const Body &b) : WorldElement(b)
00133 {
00134 mIsElastic = b.mIsElastic;
00135 material = b.material;
00136 youngMod = b.youngMod;
00137 showFC = b.showFC;
00138 mUsesFlock = b.mUsesFlock;
00139 mRenderGeometry = true;
00140 Tran = b.Tran;
00141
00142 IVRoot = b.IVRoot;
00143 IVTran = b.IVTran;
00144 IVContactIndicators = b.IVContactIndicators;
00145 #ifdef GEOMETRY_LIB
00146 IVPrimitiveRoot = b.IVPrimitiveRoot;
00147 #endif
00148 IVBVRoot = b.IVBVRoot;
00149 IVGeomRoot = b.IVGeomRoot;
00150 IVMat = b.IVMat;
00151 mGeometryFilename = b.mGeometryFilename;
00152
00153 createAxesGeometry();
00154 }
00155
00161 Body::~Body()
00162 {
00163 breakContacts();
00164 DBGP("Deleting Body: " << myName.latin1());
00165 }
00166
00175 void
00176 Body::cloneFrom(const Body* original)
00177 {
00178 mIsElastic = original->mIsElastic;
00179 youngMod = original->youngMod;
00180 material = original->material;
00181
00182
00183 virtualContactList.clear();
00184 std::list<Contact*> vc = original->getVirtualContacts();
00185 std::list<Contact*>::iterator it;
00186 for (it = vc.begin(); it!=vc.end(); it++) {
00187 VirtualContact *newContact = new VirtualContact( (VirtualContact*)(*it) );
00188 newContact->setBody(this);
00189 addVirtualContact(newContact);
00190 }
00191
00192 setRenderGeometry( original->getRenderGeometry() );
00193
00194 int numGeomChildren = original->getIVGeomRoot()->getNumChildren();
00195
00196 for (int i=0; i<numGeomChildren; i++) {
00197 IVGeomRoot->addChild( original->getIVGeomRoot()->getChild(i) );
00198 }
00199
00200 addIVMat(true);
00201
00202
00203 cloneToIvc(original);
00204 setTran(original->getTran());
00205 }
00206
00213 int
00214 Body::loadFromXml(const TiXmlElement *root, QString rootPath)
00215 {
00216
00217 const TiXmlElement* element = findXmlElement(root,"material");
00218 QString valueStr;
00219 if(element == NULL){
00220 DBGA("No material type found; using default.");
00221 material = myWorld->getMaterialIdx("wood");
00222 } else {
00223 valueStr = element->GetText();
00224 if (!valueStr.isEmpty()) {
00225 material = myWorld->getMaterialIdx(valueStr);
00226 if (material==-1) {
00227 QTWARNING("invalid material type in body file");
00228 return FAILURE;
00229 }
00230 } else{
00231 DBGA("No material type found; using default.");
00232 material = myWorld->getMaterialIdx("wood");
00233 }
00234 }
00235
00236
00237 element = findXmlElement(root,"youngs");
00238 if(element) {
00239 valueStr = element->GetText();
00240 youngMod = valueStr.toDouble();
00241 if (youngMod <= 0) {
00242 QTWARNING("invalid Young's modulus in body file");
00243 return FAILURE;
00244 }
00245 mIsElastic = true;
00246 }
00247
00248
00249 element = findXmlElement(root,"useFlockOfBirds");
00250 if(element){
00251 valueStr = element->GetText();
00252 mBirdNumber = valueStr.toDouble();
00253 mUsesFlock = true;
00254 DBGA("Object using Flock of Birds sensor " << mBirdNumber);
00255 }
00256
00257
00258 element = findXmlElement(root,"geometryFile");
00259 if (!element) {
00260 QTWARNING("Geometry file information missing");
00261 return FAILURE;
00262 } else {
00263
00264 mGeometryFilename = element->GetText();
00265 valueStr = rootPath + mGeometryFilename;
00266 mGeometryFileType = element->Attribute("type");
00267
00268 if (mGeometryFileType.isNull()||mGeometryFileType.isEmpty()) mGeometryFileType="Inventor";
00269 int result;
00270 if (mGeometryFileType=="Inventor") {
00271 result = loadGeometryIV(valueStr);
00272 } else if (mGeometryFileType=="off") {
00273 result = loadGeometryOFF(valueStr);
00274 } else if (mGeometryFileType=="ply") {
00275 result = loadGeometryPLY(valueStr);
00276 } else {
00277 DBGA("Unknown geometry file type: " << mGeometryFileType.latin1());
00278 result = FAILURE;
00279 }
00280 if (result == FAILURE) {
00281 QTWARNING("Failed to open geometry file: " + valueStr);
00282 return FAILURE;
00283 }
00284 }
00285
00286
00287 element = findXmlElement(root,"geometryScaling");
00288 if (element) {
00289 valueStr = element->GetText();
00290 double scale = valueStr.toDouble();
00291 if (scale <= 0) {
00292 DBGA("Scale geometry: negative scale found");
00293 return FAILURE;
00294 }
00295 SoTransform* IVScaleTran = new SoTransform;
00296 IVScaleTran->scaleFactor.setValue(scale, scale, scale);
00297 IVGeomRoot->insertChild(IVScaleTran, 0);
00298 }
00299
00300
00301
00302
00303 element = findXmlElement(root,"geometryOffset");
00304 if (element) {
00305 const TiXmlElement* transformElement = findXmlElement(element,"transform");
00306 if(!transformElement){
00307 DBGA("Geometry offset field missing transform information");
00308 return FAILURE;
00309 }
00310 transf offsetTran;
00311 if(!getTransform(transformElement, offsetTran)){
00312 DBGA("Geometry offset field: failed to parse transform");
00313 return FAILURE;
00314 }
00315 SoTransform *IVOffsetTran = new SoTransform;
00316 offsetTran.toSoTransform(IVOffsetTran);
00317 IVGeomRoot->insertChild(IVOffsetTran, 0);
00318 }
00319
00320 return SUCCESS;
00321 }
00322
00323 int
00324 Body::saveToXml(QTextStream& xml){
00325 xml<<"\t\t\t<material>"<<myWorld->getMaterialName(material).latin1()<<"</material>"<<endl;
00326 if(youngMod>0)
00327 xml<<"\t\t\t<youngs>"<<youngMod<<"</youngs>"<<endl;
00328 if(mUsesFlock){
00329 xml<<"\t\t\t<useFlockOfBirds>"<<mBirdNumber<<"</useFlockOfBirds>"<<endl;
00330 }
00331 xml<<"\t\t\t<geometryFile type = \""<<mGeometryFileType.latin1()<<"\">"
00332 <<mGeometryFilename.latin1()<<"</geometryFile>"<<endl;
00333 return SUCCESS;
00334 }
00335
00336
00347 int
00348 Body::load(const QString &filename)
00349 {
00350 QString fileType = filename.section('.',-1,-1);
00351 QString xmlFilename;
00352 if (fileType == "xml"){
00353
00354 xmlFilename = filename;
00355 } else {
00356
00357 DBGA("Loading geometry file with boilerplate XML file");
00358 xmlFilename = QString(getenv("GRASPIT")) + QString("/models/objects/default.xml");
00359 }
00360
00361
00362 myFilename = relativePath(filename, getenv("GRASPIT"));
00363 if (myName.isEmpty() || myName == "unnamed") {
00364 setName(filename.section('/',-1).section('.',0,0));
00365 }
00366
00367
00368 TiXmlDocument doc(xmlFilename);
00369 if(doc.LoadFile()==false){
00370 QTWARNING("Could not open " + xmlFilename);
00371 return FAILURE;
00372 }
00373 if (fileType != "xml") {
00374
00375 QString relFilename = relativePath(filename, QString(getenv("GRASPIT")) +
00376 QString("/models/objects/"));
00377 TiXmlElement * element = new TiXmlElement("geometryFile");
00378 if (fileType=="iv" || fileType=="wrl") {
00379 element->SetAttribute("type","Inventor");
00380 } else if (fileType=="off") {
00381 element->SetAttribute("type","off");
00382 } else if (fileType=="ply") {
00383 element->SetAttribute("type","ply");
00384 }
00385 TiXmlText * text = new TiXmlText( relFilename );
00386 element->LinkEndChild(text);
00387 doc.RootElement()->LinkEndChild(element);
00388 }
00389
00390 QString root = xmlFilename.section('/',0,-2,QString::SectionIncludeTrailingSep);
00391 if (loadFromXml(doc.RootElement(), root) != SUCCESS) {
00392 return FAILURE;
00393 }
00394
00395 addIVMat();
00396 return SUCCESS;
00397 }
00398
00404 int
00405 Body::loadGeometryIV(const QString &filename)
00406 {
00407 SoInput myInput;
00408 if (!myInput.openFile(filename.latin1())) {
00409 QTWARNING("Could not open Inventor file " + filename);
00410 return FAILURE;
00411 }
00412
00413
00414 SoGroup *fileGeomRoot;
00415 if (myInput.isFileVRML2()) {
00416 fileGeomRoot = SoDB::readAllVRML(&myInput);
00417 } else {
00418 fileGeomRoot = SoDB::readAll(&myInput);
00419 }
00420 myInput.closeFile();
00421 if (fileGeomRoot == NULL) {
00422 QTWARNING("A problem occurred while reading Inventor file" + filename);
00423 return FAILURE;
00424 }
00425
00426 IVGeomRoot->addChild(fileGeomRoot);
00427 return SUCCESS;
00428 }
00429
00430
00431 using std::string;
00432 using std::getline;
00433 using std::ifstream;
00434 using std::istringstream;
00435 using std::pow;
00436 using std::sqrt;
00437 using std::vector;
00438
00439
00441
00442 bool GetOffLine(ifstream* file, istringstream* line)
00443 {
00444 string buffer;
00445 if (!file->good()) return false;
00446 getline(*file, buffer);
00447
00448 buffer = buffer.substr(buffer.find_first_not_of(" \t\n\f\r"),
00449 buffer.find_first_of("#"));
00450 if (!buffer.empty()) {
00451 line->clear();
00452 line->str(buffer);
00453 return true;
00454 }
00455 return GetOffLine(file, line);
00456 }
00457
00459 int OFFReadFailure() {
00460 DBGA("OFF reader failure");
00461 return FAILURE;
00462 }
00463
00464
00470 int
00471 Body::loadGeometryOFF(const QString& filename) {
00472 ifstream file(filename.toStdString().c_str());
00473 istringstream line;
00474
00475
00476 if (!GetOffLine(&file, &line)) return OFFReadFailure();
00477
00478
00479
00480 if (!GetOffLine(&file, &line)) return OFFReadFailure();
00481 long num_vertices, num_faces;
00482 line >> num_vertices >> num_faces;
00483 if (line.fail()) return OFFReadFailure();
00484
00485 SbVec3f* vertices = new SbVec3f[num_vertices];
00486 std::vector<int32_t> face_indices;
00487
00488 for (long vertex = 0; vertex < num_vertices; ++vertex) {
00489 if (!GetOffLine(&file, &line)) return OFFReadFailure();
00490 float x, y, z;
00491 line >> x >> y >> z;
00492 if (line.fail()) return OFFReadFailure();
00493 vertices[vertex].setValue(x, y, z);
00494 }
00495
00496 for (long face = 0; face < num_faces; ++face) {
00497 if (!GetOffLine(&file, &line)) return OFFReadFailure();
00498 int num_points, vertex_index;
00499 line >> num_points;
00500
00501 for (int point = 0; point < num_points; ++point) {
00502 line >> vertex_index;
00503 face_indices.push_back(vertex_index);
00504
00505 }
00506 if (line.fail()) return OFFReadFailure();
00507 face_indices.push_back(SO_END_FACE_INDEX);
00508 }
00509
00510
00511 SoCoordinate3* coords = new SoCoordinate3;
00512 coords->point.setValues(0, num_vertices, vertices);
00513 SoIndexedFaceSet* ifs = new SoIndexedFaceSet;
00514 ifs->coordIndex.setValues(0, face_indices.size(), &(face_indices[0]));
00515 this->IVGeomRoot->addChild(coords);
00516 this->IVGeomRoot->addChild(ifs);
00517
00518 DBGA("OFF reader success");
00519 return SUCCESS;
00520 }
00521
00530 int
00531 Body::loadGeometryPLY(const QString& filename)
00532 {
00533 #ifndef PLY_READER
00534 DBGA("PLY Reader not installed; can not read file");
00535 return FAILURE;
00536 #else
00537 PLYModelLoader loader;
00538 std::vector<position> vertices;
00539 std::vector<int> triangles;
00540 if (loader.readFromFile(filename.toStdString(), vertices, triangles)) {
00541 DBGA("PLY loader error");
00542 return FAILURE;
00543 }
00544 return loadGeometryMemory(vertices, triangles);
00545 #endif
00546 }
00547
00548 int
00549 Body::loadGeometryMemory(const std::vector<position> &vertices, const std::vector<int> &triangles)
00550 {
00551 int num_vertices = vertices.size();
00552 SbVec3f* sbVertices = new SbVec3f[num_vertices];
00553 for (size_t i=0; i<vertices.size(); i++) {
00554 sbVertices[i].setValue(vertices[i].x(), vertices[i].y(), vertices[i].z());
00555 }
00556 SoCoordinate3* coords = new SoCoordinate3;
00557 coords->point.setValues(0, num_vertices, sbVertices);
00558 std::vector<int32_t> face_indices;
00559 for (size_t i=0; i<triangles.size(); i++) {
00560 face_indices.push_back(triangles[i]);
00561 if (i%3==2) {
00562 face_indices.push_back(SO_END_FACE_INDEX);
00563 }
00564 }
00565 SoIndexedFaceSet* ifs = new SoIndexedFaceSet;
00566 ifs->coordIndex.setValues(0, face_indices.size(), &(face_indices[0]));
00567 this->IVGeomRoot->addChild(coords);
00568 this->IVGeomRoot->addChild(ifs);
00569 return SUCCESS;
00570 }
00571
00579 void
00580 Body::addIVMat(bool clone)
00581 {
00582 IVMat = new SoMaterial;
00583 IVMat->diffuseColor.setIgnored(true);
00584 IVMat->ambientColor.setIgnored(true);
00585 IVMat->specularColor.setIgnored(true);
00586 IVMat->emissiveColor.setIgnored(true);
00587 IVMat->shininess.setIgnored(true);
00588
00589 if (clone) {
00590
00591 IVGeomRoot->addChild(IVMat);
00592 } else {
00593 SoSearchAction *sa = new SoSearchAction;
00594 sa->setInterest(SoSearchAction::ALL);
00595 sa->setType(SoMaterial::getClassTypeId());
00596 sa->apply(IVGeomRoot);
00597
00598 if (sa->getPaths().getLength() == 0) {
00599 IVGeomRoot->insertChild(IVMat,0);
00600 } else {
00601 for (int i=0; i<sa->getPaths().getLength(); i++) {
00602 SoGroup *g = (SoGroup *)sa->getPaths()[i]->getNodeFromTail(1);
00603 if (((SoMaterial *)sa->getPaths()[i]->getTail())->transparency[0] == 0.0f) {
00604 g->insertChild(IVMat,sa->getPaths()[i]->getIndexFromTail(0)+1);
00605 }
00606 }
00607 }
00608 delete sa;
00609 }
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620 }
00621
00624 void
00625 Body::addToIvc(bool ExpectEmpty)
00626 {
00627 myWorld->getCollisionInterface()->addBody(this, ExpectEmpty);
00628 myWorld->getCollisionInterface()->setBodyTransform(this, Tran);
00629 }
00630
00635 void
00636 Body::cloneToIvc(const Body *original)
00637 {
00638 myWorld->getCollisionInterface()->cloneBody(this, original);
00639 myWorld->getCollisionInterface()->setBodyTransform(this, Tran);
00640 }
00641
00642 void
00643 Body::setDefaultViewingParameters()
00644 {
00645 showFC = false;
00646 showVC = false;
00647 setTransparency(0.0);
00648 }
00649
00653 void
00654 Body::initializeIV()
00655 {
00656 IVRoot = new SoSeparator;
00657 IVTran = new SoTransform;
00658 IVRoot->insertChild(IVTran,0);
00659
00660
00661
00662 createAxesGeometry();
00663
00664 IVContactIndicators = new SoSeparator;
00665 IVRoot->addChild(IVContactIndicators);
00666
00667 #ifdef GEOMETRY_LIB
00668 IVPrimitiveRoot = new SoSeparator;
00669 IVRoot->addChild(IVPrimitiveRoot);
00670 #endif
00671
00672 IVBVRoot = new SoSeparator;
00673 IVRoot->addChild(IVBVRoot);
00674
00675 IVGeomRoot = new SoSeparator;
00676 IVRoot->addChild(IVGeomRoot);
00677 }
00678
00682 void
00683 Body::setBVGeometry(const std::vector<BoundingBox> &bvs)
00684 {
00685 IVBVRoot->removeAllChildren();
00686 int mark = 0;
00687 for (int i=0; i<(int)bvs.size(); i++) {
00688 SoSeparator *bvSep = new SoSeparator;
00689
00690 SoMaterial *bvMat = new SoMaterial;
00691 bvSep->addChild(bvMat);
00692 float r,g,b;
00693
00694 r = ((float)rand()) / RAND_MAX;
00695 g = ((float)rand()) / RAND_MAX;
00696 b = ((float)rand()) / RAND_MAX;
00697
00698
00699 if (bvs[i].mMark) {
00700 mark++;
00701 r = 0.8f; g=0.0f; b=0.0f;
00702 } else {
00703 r = g = b = 0.5f;
00704 }
00705
00706 bvMat->diffuseColor = SbColor(r,g,b);
00707 bvMat->ambientColor = SbColor(r,g,b);
00708 bvMat->transparency = 0.5;
00709
00710 SoTransform* bvTran = new SoTransform;
00711 bvs[i].getTran().toSoTransform(bvTran);
00712 bvSep->addChild(bvTran);
00713
00714
00715
00716 SoCube *bvBox = new SoCube;
00717 bvBox->width = 2 * bvs[i].halfSize.x();
00718 bvBox->height = 2 * bvs[i].halfSize.y();
00719 bvBox->depth = 2 * bvs[i].halfSize.z();
00720 bvSep->addChild(bvBox);
00721
00722
00723
00724
00725
00726
00727
00728
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739 IVBVRoot->addChild(bvSep);
00740 }
00741 DBGA("Setting bv geom: " << bvs.size() << " boxes. Marked: " << mark);
00742 }
00743
00748 float
00749 Body::getTransparency() const
00750 {
00751 return IVMat->transparency[0];
00752 }
00753
00754
00760 void
00761 Body::setTransparency(float t)
00762 {
00763 IVMat->transparency = t;
00764 }
00765
00766
00771 void
00772 Body::setMaterial(int mat)
00773 {
00774 std::list<Contact *>::iterator cp;
00775
00776 material = mat;
00777 if (showFC || showVC) IVContactIndicators->removeAllChildren();
00778
00779 for (cp=contactList.begin();cp!=contactList.end();cp++) {
00780 (*cp)->updateCof();
00781 (*cp)->getMate()->updateCof();
00782 (*cp)->getBody2()->redrawFrictionCones();
00783 }
00784 redrawFrictionCones();
00785 }
00786
00787
00791 void
00792 Body::showFrictionCones(bool on, int vc)
00793 {
00794 showFC = on;
00795 if (vc==1) showVC = true;
00796 else if (vc==2) showVC = false;
00797 redrawFrictionCones();
00798 }
00799
00800
00804 void
00805 Body::redrawFrictionCones()
00806 {
00807 std::list<Contact *>::iterator cp;
00808
00809 IVContactIndicators->removeAllChildren();
00810 if (showFC) {
00811 for (cp=contactList.begin();cp!=contactList.end();cp++)
00812 IVContactIndicators->addChild( (*cp)->getVisualIndicator() );
00813 }
00814 if (showVC) {
00815 for (cp = virtualContactList.begin(); cp!=virtualContactList.end(); cp++)
00816 IVContactIndicators->addChild( ( (VirtualContact*)(*cp) )->getVisualIndicator() );
00817 }
00818 }
00819
00823 void
00824 Body::setRenderGeometry(bool s)
00825 {
00826 assert(IVTran);
00827 mRenderGeometry = s;
00828 if (!s) {
00829
00830
00831
00832
00833
00834 IVTran->translation.enableNotify(false);
00835 IVTran->rotation.enableNotify(false);
00836 } else {
00837
00838
00839
00840
00841
00842 IVTran->translation.enableNotify(true);
00843 IVTran->rotation.enableNotify(true);
00844 }
00845
00846 }
00847
00852 int
00853 Body::setTran(transf const &tr)
00854 {
00855 if (tr == Tran) return SUCCESS;
00856 breakContacts();
00857
00858 if (!myWorld->wasModified() && tr != Tran) {
00859 myWorld->setModified();
00860 }
00861 Tran = tr;
00862 myWorld->getCollisionInterface()->setBodyTransform(this, Tran);
00863 if (IVTran) {
00864 Tran.toSoTransform(IVTran);
00865 }
00866 return SUCCESS;
00867 }
00868
00869
00874 bool
00875 Body::contactsPreventMotion(const transf& motion) const
00876 {
00877 std::list<Contact *>::iterator cp;
00878 std::list<Contact *> contactList;
00879
00880 contactList = getContacts();
00881 for (cp=contactList.begin();cp!=contactList.end();cp++) {
00882 if ((*cp)->preventsMotion(motion)) {
00883 return true;
00884 }
00885 }
00886 return false;
00887 }
00888
00894 void
00895 Body::breakContacts()
00896 {
00897 std::list<Contact *>::iterator cp;
00898
00899 if (!contactList.empty()) {
00900 setContactsChanged();
00901 for (cp=contactList.begin();cp!=contactList.end();cp++) {
00902 delete *cp; *cp = NULL;
00903 }
00904 contactList.clear();
00905 }
00906 numContacts = 0;
00907
00908 for (cp = prevContactList.begin(); cp!=prevContactList.end(); cp++) {
00909 if ( (*cp)->getMate() != NULL ) {
00910 (*cp)->getMate()->setMate(NULL);
00911 }
00912 (*cp)->setMate(NULL);
00913 delete *cp; *cp = NULL;
00914 }
00915 prevContactList.clear();
00916
00917 if (showFC)
00918 IVContactIndicators->removeAllChildren();
00919 }
00920
00922 int
00923 Body::loadContactData(QString fn)
00924 {
00925 FILE *fp = fopen(fn.latin1(), "r");
00926 if (!fp) {
00927 fprintf(stderr,"Could not open filename %s\n",fn.latin1());
00928 return FAILURE;
00929 }
00930 int numContacts;
00931 VirtualContactOnObject *newContact;
00932 fscanf(fp,"%d",&numContacts);
00933
00934 breakVirtualContacts();
00935 for (int i=0; i<numContacts; i++) {
00936 newContact = new VirtualContactOnObject();
00937 newContact->readFromFile(fp);
00938 newContact->setBody( this );
00939 ((Contact*)newContact)->computeWrenches();
00940 addVirtualContact( newContact );
00941 }
00942 fclose(fp);
00943 return SUCCESS;
00944 }
00947 void
00948 Body::breakVirtualContacts()
00949 {
00950 std::list<Contact *>::iterator cp;
00951 for (cp = virtualContactList.begin(); cp!= virtualContactList.end(); cp++) {
00952 delete *cp;
00953 }
00954
00955 if (showVC) {
00956 IVContactIndicators->removeAllChildren();
00957 }
00958 virtualContactList.clear();
00959 }
00960
00965 void
00966 Body::resetContactList()
00967 {
00968 std::list<Contact *>::iterator cp;
00969 for (cp = prevContactList.begin(); cp!=prevContactList.end(); cp++) {
00970 if ( (*cp)->getMate() != NULL ) {
00971 (*cp)->getMate()->setMate(NULL);
00972 }
00973 (*cp)->setMate(NULL);
00974 delete *cp; *cp = NULL;
00975 }
00976 prevContactList.clear();
00977 if (!contactList.empty()) {
00978 setContactsChanged();
00979 for (cp=contactList.begin();cp!=contactList.end();cp++) {
00980 prevContactList.push_back(*cp);
00981 }
00982 contactList.clear();
00983 }
00984 numContacts = 0;
00985 if (showFC)
00986 IVContactIndicators->removeAllChildren();
00987 }
00988
00994 void
00995 Body::addContact(Contact *c)
00996 {
00997 setContactsChanged();
00998 contactList.push_back(c);
00999 numContacts++;
01000 if (showFC) {
01001 assert(IVContactIndicators->getNumChildren() > numContacts-2);
01002 IVContactIndicators->insertChild( c->getVisualIndicator(), numContacts-1 );
01003 }
01004 }
01005
01010 int
01011 Body::getNumContacts(Body *b) const
01012 {
01013 if (!b) return numContacts;
01014 int c = 0;
01015 std::list<Contact*>::const_iterator it;
01016 for (it = contactList.begin(); it!=contactList.end(); it++) {
01017 if ( (*it)->getBody2() == b) c++;
01018 }
01019 return c;
01020 }
01021
01026 std::list<Contact*>
01027 Body::getContacts(Body *b) const
01028 {
01029 if (!b) return contactList;
01030 std::list<Contact*> contacts;
01031 std::list<Contact*>::const_iterator it;
01032 for (it = contactList.begin(); it!=contactList.end(); it++) {
01033 if ( (*it)->getBody2() == b) contacts.push_back(*it);
01034 }
01035 return contacts;
01036 }
01037
01039 void
01040 Body::addVirtualContact(Contact *c)
01041 {
01042 setContactsChanged();
01043 virtualContactList.push_back(c);
01044 if (showVC)
01045 IVContactIndicators->addChild( c->getVisualIndicator() );
01046 }
01047
01054 Contact*
01055 Body::checkContactInheritance(Contact *c)
01056 {
01057 std::list<Contact *>::iterator cp;
01058 bool inheritance = false;
01059 for (cp = prevContactList.begin(); cp != prevContactList.end(); cp++) {
01060 if ( (*cp)->getBody1() != c->getBody1() )
01061 continue;
01062 if ( (*cp)->getBody2() != c->getBody2() )
01063 continue;
01064 vec3 d = (*cp)->getPosition() - c->getPosition();
01065 if (d.len() > Contact::INHERITANCE_THRESHOLD )
01066 continue;
01067 vec3 n1 = (*cp)->getNormal();
01068 vec3 n2 = c->getNormal();
01069 double theta = n1 % n2;
01070 if ( theta < Contact::INHERITANCE_ANGULAR_THRESHOLD )
01071 continue;
01072 inheritance = true;
01073 break;
01074 }
01075 if (!inheritance)
01076 return NULL;
01077 return (*cp);
01078 }
01079
01080
01085 void
01086 Body::removeContact(Contact *c)
01087 {
01088 int i;
01089 std::list<Contact *>::iterator cp;
01090
01091 setContactsChanged();
01092
01093 if (showFC) {
01094 for (cp=contactList.begin(),i=0;cp!=contactList.end();cp++,i++)
01095 if (*cp == c) {
01096 contactList.erase(cp);
01097 break;
01098 }
01099 IVContactIndicators->removeChild(i);
01100 }
01101 else contactList.remove(c);
01102
01103 delete c;
01104 numContacts--;
01105 }
01106
01110 void Body::removePrevContact(Contact *c)
01111 {
01112 prevContactList.remove(c);
01113 c->setMate(NULL);
01114 delete c;
01115 }
01116
01120 QTextStream&
01121 operator<<(QTextStream &os, const Body &b)
01122 {
01123 os << b.myFilename << endl;
01124 os << b.myWorld->getMaterialName(b.material) << endl;
01125 return os;
01126 }
01127
01128
01130 void addTriangleCallBack(void* info, SoCallbackAction * action,
01131 const SoPrimitiveVertex * v1,
01132 const SoPrimitiveVertex * v2,
01133 const SoPrimitiveVertex * v3)
01134 {
01135 std::vector<Triangle> *triangles = (std::vector<Triangle>*) info;
01136
01137 SbVec3f p1, p2, p3;
01138 SbMatrix mm = action->getModelMatrix();
01139
01140
01141 mm.multVecMatrix( v1->getPoint(), p1 );
01142 mm.multVecMatrix( v2->getPoint(), p2 );
01143 mm.multVecMatrix( v3->getPoint(), p3 );
01144
01145
01146 if ((p1 == p2) || (p2 == p3) || (p1 == p3)) return;
01147
01148 position nv1(p1[0], p1[1], p1[2]);
01149 position nv2(p2[0], p2[1], p2[2]);
01150 position nv3(p3[0], p3[1], p3[2]);
01151 Triangle newTri(nv1,nv2,nv3);
01152 triangles->push_back( newTri );
01153 }
01154
01156 void addVertexCallBack(void* info, SoCallbackAction * action, const SoPrimitiveVertex * v1)
01157 {
01158 std::vector<position> *vertices = (std::vector<position>*) info;
01159 SbVec3f p1;
01160 SbMatrix mm = action->getModelMatrix();
01161
01162 mm.multVecMatrix( v1->getPoint(), p1 );
01163 position nv1(p1[0], p1[1], p1[2]);
01164 vertices->push_back( nv1 );
01165 }
01166
01168 void addVerticesFromTriangleCallBack(void* info, SoCallbackAction * action,
01169 const SoPrimitiveVertex * v1,
01170 const SoPrimitiveVertex * v2,
01171 const SoPrimitiveVertex * v3)
01172 {
01173 std::vector<position> *vertices = (std::vector<position>*) info;
01174 SbVec3f p1, p2, p3;
01175 SbMatrix mm = action->getModelMatrix();
01176
01177 mm.multVecMatrix( v1->getPoint(), p1 );
01178 mm.multVecMatrix( v2->getPoint(), p2 );
01179 mm.multVecMatrix( v3->getPoint(), p3 );
01180 vertices->push_back(position(p1[0], p1[1], p1[2]));
01181 vertices->push_back(position(p2[0], p2[1], p2[2]));
01182 vertices->push_back(position(p3[0], p3[1], p3[2]));
01183 }
01184
01186 void
01187 Body::getGeometryTriangles(std::vector<Triangle> *triangles) const
01188 {
01189 SoCallbackAction ca;
01190 ca.addTriangleCallback(SoShape::getClassTypeId(), addTriangleCallBack, triangles);
01191 ca.apply(getIVGeomRoot());
01192 }
01193
01198 void
01199 Body::getGeometryVertices(std::vector<position> *vertices) const
01200 {
01201 SoCallbackAction ca;
01202
01203
01204
01205 ca.addTriangleCallback(SoShape::getClassTypeId(), addVerticesFromTriangleCallBack, vertices);
01206 ca.apply(getIVGeomRoot());
01207 }
01208
01212 void Body::createAxesGeometry()
01213 {
01214 IVWorstCase = new SoSeparator;
01215 IVAxes = new SoSwitch;
01216 if (graspItGUI) {
01217 SoSeparator *axesSep = new SoSeparator;
01218 axesTranToCOG = new SoTranslation;
01219 axesTranToCOG->translation.setValue(0,0,0);
01220 axesSep->addChild(axesTranToCOG);
01221 axesSep->addChild(IVWorstCase);
01222
01223 axesScale = new SoScale;
01224 axesScale->scaleFactor = SbVec3f(1,1,1);
01225 axesSep->addChild(axesScale);
01226 axesSep->addChild(graspItGUI->getIVmgr()->getPointers()->getChild(2));
01227 IVAxes->addChild(axesSep);
01228 }
01229 if (!IVRoot) IVRoot = new SoSeparator;
01230
01231 IVRoot->addChild(IVAxes);
01232 }
01233
01235
01237
01242 DynamicBody::~DynamicBody()
01243 {
01244 if (dynJoint) delete dynJoint;
01245 }
01246
01247 void
01248 DynamicBody::init()
01249 {
01250 maxRadius = 0.0; mass = 0.0;
01251 showAx = showDynCF = false;
01252 fixed = false; dynJoint=NULL; dynamicsComputedFlag = false;
01253 useDynamics = true;
01254 bbox_min = vec3(-1e+6,-1e+6,-1e+6);
01255 bbox_max = vec3(1e+6,1e+6,1e+6);
01256 CoG.set(0.0,0.0,0.0);
01257 for(int i=0; i<9; i++) {
01258 I[i] = 0.0;
01259 }
01260 resetDynamics();
01261 }
01262
01267 void
01268 DynamicBody::resetDynamics()
01269 {
01270 resetExtWrenchAcc();
01271 for (int i=0; i<6; i++) {
01272 a[i] = 0.0;
01273 v[i] = 0.0;
01274 }
01275 Quaternion quat = Tran.rotation();
01276 vec3 cogOffset = quat * (CoG-position::ORIGIN);
01277 q[0] = Tran.translation().x()+cogOffset.x();
01278 q[1] = Tran.translation().y()+cogOffset.y();
01279 q[2] = Tran.translation().z()+cogOffset.z();
01280 q[3] = quat.w;
01281 q[4] = quat.x;
01282 q[5] = quat.y;
01283 q[6] = quat.z;
01284 }
01285
01290 DynamicBody::DynamicBody(World *w, const char *name) : Body(w,name)
01291 {
01292 init();
01293 }
01294
01300 DynamicBody::DynamicBody(const Body &b, double m) : Body(b)
01301 {
01302 init();
01303 position defCoG;
01304 double defI[9];
01305 computeDefaultMassProp(defCoG, defI);
01306 setMass(m);
01307 setCoG(defCoG);
01308 setInertiaMatrix(defI);
01309 setMaxRadius(computeDefaultMaxRadius());
01310
01311 setTran(b.getTran());
01312 }
01313
01317 void
01318 DynamicBody::cloneFrom(const DynamicBody *original)
01319 {
01320 Body::cloneFrom(original);
01321 setMass(original->mass);
01322 setCoG(original->CoG);
01323 setInertiaMatrix(original->I);
01324 setMaxRadius(original->maxRadius);
01325 }
01326
01327 int
01328 DynamicBody::loadFromXml(const TiXmlElement *root, QString rootPath)
01329 {
01330 if (Body::loadFromXml(root, rootPath)==FAILURE) {
01331 return FAILURE;
01332 }
01333 double loadMass;
01334 bool overrideI, overrideCog;
01335 QString valueStr;
01336
01337 const TiXmlElement* element = findXmlElement(root, "mass");
01338 if(element == NULL){
01339 loadMass = defaultMass;
01340 DBGA("Using default mass");
01341 } else{
01342 valueStr = element->GetText();
01343 loadMass = valueStr.toDouble();
01344 if (loadMass <= 0) {
01345 QTWARNING("invalid mass in dynamic body file: " + myFilename);
01346 return FAILURE;
01347 }
01348 }
01349 element = findXmlElement(root, "cog");
01350 position loadCog;
01351 if(element == NULL){
01352 overrideCog = false;
01353 } else{
01354 overrideCog = true;
01355 valueStr = element->GetText();
01356 valueStr = valueStr.simplifyWhiteSpace();
01357 QStringList l;
01358 l = QStringList::split(' ', valueStr.stripWhiteSpace());
01359 if(l.count()!=3){
01360 QTWARNING("Invalid Center of Gravity Input");
01361 return FAILURE;
01362 }
01363 double x,y,z;
01364 x = l[0].toDouble();
01365 y = l[1].toDouble();
01366 z = l[2].toDouble();
01367 loadCog = position(x,y,z);
01368 }
01369 double loadI[9];
01370 element = findXmlElement(root, "inertia_matrix");
01371 if(element == NULL){
01372 overrideI = false;
01373 } else{
01374 overrideI = true;
01375 valueStr = element->GetText();
01376 valueStr = valueStr.simplifyWhiteSpace();
01377 QStringList l;
01378 l = QStringList::split(' ', valueStr.stripWhiteSpace());
01379 if(l.count()!=9){
01380 QTWARNING("Invalid Inertia Matrix Input");
01381 return FAILURE;
01382 }
01383 loadI[0]=l[0].toDouble();
01384 loadI[1]=l[1].toDouble();
01385 loadI[2]=l[2].toDouble();
01386 loadI[3]=l[3].toDouble();
01387 loadI[4]=l[4].toDouble();
01388 loadI[5]=l[5].toDouble();
01389 loadI[6]=l[6].toDouble();
01390 loadI[7]=l[7].toDouble();
01391 loadI[8]=l[8].toDouble();
01392 }
01393 if (!overrideI || !overrideCog) {
01394 position defaultCog;
01395 double defaultI[9];
01396 computeDefaultMassProp(defaultCog, defaultI);
01397 if (!overrideI) {
01398 memcpy(loadI, defaultI, 9*sizeof(double));
01399 DBGA("Using default inertia matrix");
01400 }
01401 if (!overrideCog) {
01402 loadCog = defaultCog;
01403 DBGA("Using default center of gravity");
01404 }
01405 }
01406 setMass(loadMass);
01407 setCoG(loadCog);
01408 setInertiaMatrix(loadI);
01409 setMaxRadius(computeDefaultMaxRadius());
01410 return SUCCESS;
01411 }
01412
01417 void
01418 DynamicBody::setDefaultDynamicParameters()
01419 {
01420 position defaultCog;
01421 double defaultI[9];
01422 computeDefaultMassProp(defaultCog, defaultI);
01423 setCoG(defaultCog);
01424 setInertiaMatrix(defaultI);
01425 }
01426
01427 int
01428 DynamicBody::saveToXml(QTextStream &xml){
01429 if(Body::saveToXml(xml)!=SUCCESS) return FAILURE;
01430 xml<<"\t\t\t<mass>"<<mass<<"</mass>"<<endl;
01431 xml<<"\t\t\t<cog>"<<CoG.x()<<" "<<CoG.y()<<" "<<CoG.z()<<"</cog>"<<endl;
01432 xml<<"\t\t\t<inertia_matrix>";
01433 for(int i=0;i<9;i++){
01434 xml<<I[i];
01435 if(i!=8)
01436 xml<<" ";
01437 }
01438 xml<<"</inertia_matrix>"<<endl;
01439 return SUCCESS;
01440 }
01444 void
01445 DynamicBody::setCoG(const position &newCoG)
01446 {
01447 CoG = newCoG;
01448 resetDynamics();
01449
01450
01451
01452
01453 axesTranToCOG->translation.setValue(0,0,0);
01454 }
01455
01459 void
01460 DynamicBody::setMaxRadius(double maxRad)
01461 {
01462 maxRadius = maxRad;
01463 axesScale->scaleFactor = SbVec3f(maxRadius / AXES_SCALE, maxRadius / AXES_SCALE, maxRadius / AXES_SCALE);
01464 }
01465
01466 void
01467 DynamicBody::setInertiaMatrix(const double *newI)
01468 {
01469 memcpy(I, newI, 9*sizeof(double));
01470 }
01471
01472 double
01473 DynamicBody::computeDefaultMaxRadius()
01474 {
01475 std::vector<position> vertices;
01476 getGeometryVertices(&vertices);
01477 if (vertices.empty()) {
01478 DBGA("No vertices found when computing maxRadius!");
01479 }
01480 double maxRad = 0.0;
01481 for (int i=0; i<(int)vertices.size(); i++) {
01482 double tmpRadius = (CoG - vertices[i]).len();
01483 if (tmpRadius > maxRad) maxRad = tmpRadius;
01484 }
01485 return maxRad;
01486 }
01487
01492 void
01493 DynamicBody::compProjectionIntegrals(FACE &f, int A, int B)
01494 {
01495 double a0, a1, da;
01496 double b0, b1, db;
01497 double a0_2, a0_3, a0_4, b0_2, b0_3, b0_4;
01498 double a1_2, a1_3, b1_2, b1_3;
01499 double C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb;
01500 double Cab, Kab, Caab, Kaab, Cabb, Kabb;
01501 int i;
01502
01503 P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0;
01504
01505 for (i = 0; i < 3; i++) {
01506 a0 = f.verts[i][A];
01507 b0 = f.verts[i][B];
01508 a1 = f.verts[(i+1) % 3][A];
01509 b1 = f.verts[(i+1) % 3][B];
01510 da = a1 - a0;
01511 db = b1 - b0;
01512 a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0;
01513 b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0;
01514 a1_2 = a1 * a1; a1_3 = a1_2 * a1;
01515 b1_2 = b1 * b1; b1_3 = b1_2 * b1;
01516
01517 C1 = a1 + a0;
01518 Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4;
01519 Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4;
01520 Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2;
01521 Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3;
01522 Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3;
01523 Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3;
01524
01525 P1 += db*C1;
01526 Pa += db*Ca;
01527 Paa += db*Caa;
01528 Paaa += db*Caaa;
01529 Pb += da*Cb;
01530 Pbb += da*Cbb;
01531 Pbbb += da*Cbbb;
01532 Pab += db*(b1*Cab + b0*Kab);
01533 Paab += db*(b1*Caab + b0*Kaab);
01534 Pabb += da*(a1*Cabb + a0*Kabb);
01535 }
01536
01537 P1 /= 2.0;
01538 Pa /= 6.0;
01539 Paa /= 12.0;
01540 Paaa /= 20.0;
01541 Pb /= -6.0;
01542 Pbb /= -12.0;
01543 Pbbb /= -20.0;
01544 Pab /= 24.0;
01545 Paab /= 60.0;
01546 Pabb /= -60.0;
01547 }
01548
01553 void
01554 DynamicBody::compFaceIntegrals(FACE &f,int A, int B, int C)
01555 {
01556 double *n, w;
01557 double k1, k2, k3, k4;
01558
01559 compProjectionIntegrals(f,A,B);
01560
01561 w = f.w;
01562 n = f.norm;
01563 k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1;
01564
01565 Fa = k1 * Pa;
01566 Fb = k1 * Pb;
01567 Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1);
01568
01569 Faa = k1 * Paa;
01570 Fbb = k1 * Pbb;
01571 Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb
01572 + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1));
01573
01574 Faaa = k1 * Paaa;
01575 Fbbb = k1 * Pbbb;
01576 Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab
01577 + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb
01578 + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb)
01579 + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1));
01580
01581 Faab = k1 * Paab;
01582 Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb);
01583 Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb
01584 + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa));
01585 }
01586
01591 int
01592 DynamicBody::computeDefaultInertiaMatrix(std::vector<Triangle> &triangles, double *defaultI)
01593 {
01594 FACE f;
01595 double dx1,dy1,dz1,dx2,dy2,dz2;
01596 double nx, ny, nz,len;
01597 int i,A,B,C;
01598 double r[3], v1[3], v2[3], v3[3];
01599 double density;
01600
01601 f.verts[0] = v1;
01602 f.verts[1] = v2;
01603 f.verts[2] = v3;
01604
01605
01606 double T0, T1[3], T2[3], TP[3];
01607
01608 T0 = T1[0] = T1[1] = T1[2]
01609 = T2[0] = T2[1] = T2[2]
01610 = TP[0] = TP[1] = TP[2] = 0;
01611
01612
01613 for (i = 0; i < (int)triangles.size(); i++) {
01614 triangles[i].v1.get(v1);
01615 triangles[i].v2.get(v2);
01616 triangles[i].v3.get(v3);
01617
01618 dx1 = f.verts[1][0] - f.verts[0][0];
01619 dy1 = f.verts[1][1] - f.verts[0][1];
01620 dz1 = f.verts[1][2] - f.verts[0][2];
01621 dx2 = f.verts[2][0] - f.verts[1][0];
01622 dy2 = f.verts[2][1] - f.verts[1][1];
01623 dz2 = f.verts[2][2] - f.verts[1][2];
01624 nx = dy1 * dz2 - dy2 * dz1;
01625 ny = dz1 * dx2 - dz2 * dx1;
01626 nz = dx1 * dy2 - dx2 * dy1;
01627 len = sqrt(nx * nx + ny * ny + nz * nz);
01628
01629 f.norm[0] = nx / len;
01630 f.norm[1] = ny / len;
01631 f.norm[2] = nz / len;
01632 f.w = - f.norm[0] * f.verts[0][0]
01633 - f.norm[1] * f.verts[0][1]
01634 - f.norm[2] * f.verts[0][2];
01635
01636 nx = fabs(f.norm[0]);
01637 ny = fabs(f.norm[1]);
01638 nz = fabs(f.norm[2]);
01639 if (nx > ny && nx > nz) C = 0;
01640 else C = (ny > nz) ? 1 : 2;
01641 A = (C + 1) % 3;
01642 B = (A + 1) % 3;
01643
01644 compFaceIntegrals(f,A,B,C);
01645
01646 T0 += f.norm[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc));
01647
01648 T1[A] += f.norm[A] * Faa;
01649 T1[B] += f.norm[B] * Fbb;
01650 T1[C] += f.norm[C] * Fcc;
01651 T2[A] += f.norm[A] * Faaa;
01652 T2[B] += f.norm[B] * Fbbb;
01653 T2[C] += f.norm[C] * Fccc;
01654 TP[A] += f.norm[A] * Faab;
01655 TP[B] += f.norm[B] * Fbbc;
01656 TP[C] += f.norm[C] * Fcca;
01657 }
01658
01659 T1[0] /= 2; T1[1] /= 2; T1[2] /= 2;
01660 T2[0] /= 3; T2[1] /= 3; T2[2] /= 3;
01661 TP[0] /= 2; TP[1] /= 2; TP[2] /= 2;
01662
01663 r[0] = T1[0] / T0;
01664 r[1] = T1[1] / T0;
01665 r[2] = T1[2] / T0;
01666
01667 DBGP("COM: " << r[0] << " " << r[1] << " " << r[2]);
01668
01669
01670 for (int i=0; i<9; i++) {
01671 defaultI[i] = 0.0;
01672 }
01673 defaultI[0] = defaultI[3] = defaultI[6] = 1.0;
01674
01675
01676 if (r[0] != r[0]) return FAILURE;
01677 if (r[1] != r[1]) return FAILURE;
01678 if (r[2] != r[2]) return FAILURE;
01679 if (fabs(T0) < 1.0e-5) return FAILURE;
01680 if ( fabs(r[0]) > 5.0e2 || fabs(r[1]) > 5.0e2 || fabs(r[2]) > 5.0e2 ) {
01681 return FAILURE;
01682 }
01683
01684
01685 density = 1.0 / T0;
01686
01687
01688 defaultI[0] = density * (T2[1] + T2[2]);
01689 defaultI[4] = density * (T2[2] + T2[0]);
01690 defaultI[8] = density * (T2[0] + T2[1]);
01691 defaultI[1] = defaultI[3] = - density * TP[0];
01692 defaultI[5] = defaultI[7] = - density * TP[1];
01693 defaultI[6] = defaultI[2] = - density * TP[2];
01694
01695
01696 defaultI[0] -= (r[1]*r[1] + r[2]*r[2]);
01697 defaultI[4] -= (r[2]*r[2] + r[0]*r[0]);
01698 defaultI[8] -= (r[0]*r[0] + r[1]*r[1]);
01699 defaultI[1] = defaultI[3] += r[0] * r[1];
01700 defaultI[5] = defaultI[7] += r[1] * r[2];
01701 defaultI[6] = defaultI[2] += r[2] * r[0];
01702 return SUCCESS;
01703 }
01704
01709 template <class IntegrableFunction>
01710 float Gaussian7Integrate(const Triangle& triangle, IntegrableFunction integrable_function) {
01711
01712 static const float terms[8] = {
01713 0.333333333333333333333333333333333f,
01714 0.79742698535308732239802527616971f,
01715 0.10128650732345633880098736191512f,
01716 0.059715871789769820459117580973143f,
01717 0.47014206410511508977044120951345f,
01718 0.225f,
01719 0.12593918054482715259568394550018f,
01720 0.13239415278850618073764938783315f};
01721
01722 static const float barycentric_weights[7][3] = {
01723 {terms[0], terms[0], terms[0]},
01724 {terms[1], terms[2], terms[2]},
01725 {terms[2], terms[1], terms[2]},
01726 {terms[2], terms[2], terms[1]},
01727 {terms[3], terms[4], terms[4]},
01728 {terms[4], terms[3], terms[4]},
01729 {terms[4], terms[4], terms[3]}};
01730 static const float sample_weights[7] = {
01731 terms[5], terms[6], terms[6], terms[6],
01732 terms[7], terms[7], terms[7]};
01733
01734 float integration_samples[7][3] = {0};
01735 const position* vertices[3] = {&(triangle.v1), &(triangle.v2), &(triangle.v3)};
01736 for(int sample_num = 0; sample_num < 7; ++sample_num) {
01737 float* sample = integration_samples[sample_num];
01738 const float* barycentric_weight = barycentric_weights[sample_num];
01739 for (int vertex_num = 0; vertex_num < 3; ++vertex_num) {
01740 const position& vertex = *(vertices[vertex_num]);
01741 for (int coord = 0; coord < 3; ++coord) {
01742 sample[coord] += vertex[coord] * barycentric_weight[vertex_num];
01743 }
01744 }
01745 }
01746
01747 float result = 0;
01748 for (int sample = 0; sample < 7; ++sample){
01749 result += integrable_function(integration_samples[sample]) * sample_weights[sample];
01750 }
01751 return result * triangle.area();
01752 }
01753
01755 struct GetCoord {
01756 int coord;
01757 template <class T> float operator()(const T vertex) const { return vertex[coord]; }
01758 };
01759
01761 struct GetCovar {
01762 int coord_a, coord_b;
01763 float mean_a, mean_b;
01764 template <class T> float operator()(const T vertex) const {
01765 return (vertex[coord_a] - mean_a) * (vertex[coord_b] - mean_b);
01766 }
01767 };
01768
01773 int
01774 computeDefaultCoG(std::vector<Triangle> &triangles, position &defaultCoG)
01775 {
01776
01777 double center_of_mass[3] = {0, 0, 0};
01778
01779 float total_area = 0;
01780 for (std::vector<Triangle>::const_iterator triangle = triangles.begin();
01781 triangle != triangles.end(); ++triangle) {
01782 total_area += triangle->area();
01783 }
01784
01785 if (total_area != 0) {
01786
01787 float means[3] = {0};
01788
01789 GetCoord get_coord;
01790 for(unsigned i = 0; i < 3; ++i) {
01791 get_coord.coord = i;
01792 for (std::vector<Triangle>::const_iterator triangle = triangles.begin();
01793 triangle != triangles.end(); ++triangle) {
01794 means[i] += Gaussian7Integrate(*triangle, get_coord);
01795 }
01796 }
01797 for (int i = 0; i < 3; ++i) center_of_mass[i] = means[i] / total_area;
01798
01799
01800 float covars[3][3] = {0};
01801 GetCovar get_covar;
01802 for(unsigned a = 0; a < 3; ++a) {
01803 for(unsigned b = a; b < 3; ++b) {
01804 get_covar.coord_a = a;
01805 get_covar.coord_b = b;
01806 get_covar.mean_a = center_of_mass[a];
01807 get_covar.mean_b = center_of_mass[b];
01808 for (std::vector<Triangle>::const_iterator triangle = triangles.begin();
01809 triangle != triangles.end(); ++triangle) {
01810 covars[b][a] += Gaussian7Integrate(*triangle, get_covar);
01811 }
01812 }
01813 }
01814 defaultCoG.set(center_of_mass);
01815 return SUCCESS;
01816 } else {
01817 defaultCoG.set(0,0,0);
01818 return FAILURE;
01819 }
01820 }
01826 void
01827 DynamicBody::computeDefaultMassProp(position &defaultCoG, double *defaultI)
01828 {
01829 std::vector<Triangle> triangles;
01830 getGeometryTriangles(&triangles);
01831 if (triangles.empty()) {
01832 DBGA("No triangles found when computing mass properties!");
01833 defaultCoG.set(0,0,0);
01834 return;
01835 }
01836 if (computeDefaultInertiaMatrix(triangles, defaultI) == FAILURE) {
01837 DBGA("Failed to compute inertia matrix based on geometry; using identity");
01838 }
01839 for(int i=0; i<3; i++) {
01840 DBGP(defaultI[3*i] << " " << defaultI[3*i+1] << " " << defaultI[3*i+2]);
01841 }
01842 computeDefaultCoG(triangles, defaultCoG);
01843 }
01844
01845 void
01846 DynamicBody::setDefaultViewingParameters()
01847 {
01848 Body::setDefaultViewingParameters();
01849 showAx = false;
01850 }
01851
01856 void
01857 DynamicBody::showAxes(bool on)
01858 {
01859 DBGP("Show axes: " << on);
01860 if (on) IVAxes->whichChild = 0;
01861 else IVAxes->whichChild = -1;
01862 showAx = on;
01863 }
01864
01869 void
01870 DynamicBody::showDynContactForces(bool on)
01871 {
01872 showDynCF = on;
01873 }
01874
01875
01879 void
01880 DynamicBody::resetExtWrenchAcc()
01881 {
01882 extWrenchAcc[0] = extWrenchAcc[1] = extWrenchAcc[2] = 0.0;
01883 extWrenchAcc[3] = extWrenchAcc[4] = extWrenchAcc[5] = 0.0;
01884 }
01885
01889 void
01890 DynamicBody::addExtWrench(double *extW){
01891 extWrenchAcc[0] += extW[0];
01892 extWrenchAcc[1] += extW[1];
01893 extWrenchAcc[2] += extW[2];
01894 extWrenchAcc[3] += extW[3];
01895 extWrenchAcc[4] += extW[4];
01896 extWrenchAcc[5] += extW[5];
01897 }
01898
01903 void
01904 DynamicBody::addForce(vec3 force)
01905 {
01906 extWrenchAcc[0] += force[0];
01907 extWrenchAcc[1] += force[1];
01908 extWrenchAcc[2] += force[2];
01909
01910 }
01911
01916 void
01917 DynamicBody::addTorque(vec3 torque)
01918 {
01919 extWrenchAcc[3] += torque[0];
01920 extWrenchAcc[4] += torque[1];
01921 extWrenchAcc[5] += torque[2];
01922 DBGP("Adding torque "<< torque);
01923 }
01924
01925
01926
01927
01928
01929 void
01930 DynamicBody::addRelTorque(vec3 torque)
01931 {
01932 vec3 worldTorque;
01933 DBGP("Adding rel torque "<< torque);
01934 worldTorque = Tran.rotation() * torque;
01935 extWrenchAcc[3] += worldTorque[0];
01936 extWrenchAcc[4] += worldTorque[1];
01937 extWrenchAcc[5] += worldTorque[2];
01938 DBGP(" world torque "<< worldTorque);
01939 }
01940
01941
01947 void
01948 DynamicBody::addForceAtPos(vec3 force,position pos)
01949 {
01950 vec3 worldTorque;
01951 vec3 worldPos;
01952 worldPos = (pos - CoG * Tran);
01953
01954 worldTorque = worldPos * force;
01955 extWrenchAcc[0] += force[0];
01956 extWrenchAcc[1] += force[1];
01957 extWrenchAcc[2] += force[2];
01958
01959 extWrenchAcc[3] += worldTorque[0];
01960 extWrenchAcc[4] += worldTorque[1];
01961 extWrenchAcc[5] += worldTorque[2];
01962 }
01963
01964
01970 void
01971 DynamicBody::addForceAtRelPos(vec3 force,position pos)
01972 {
01973 vec3 worldForce;
01974 vec3 worldTorque;
01975
01976 worldForce = Tran.rotation() * force;
01977 worldTorque = Tran.rotation() * ((pos - CoG) * force);
01978 extWrenchAcc[0] += worldForce[0];
01979 extWrenchAcc[1] += worldForce[1];
01980 extWrenchAcc[2] += worldForce[2];
01981
01982 extWrenchAcc[3] += worldTorque[0];
01983 extWrenchAcc[4] += worldTorque[1];
01984 extWrenchAcc[5] += worldTorque[2];
01985 DBGP("Adding rel force "<< force << " at " << pos);
01986 DBGP(" world torque "<< worldTorque << " worldForce " << worldForce);
01987 }
01988
01989
01990 bool
01991 DynamicBody::setPos(const double *new_q)
01992 {
01993 double norm;
01994
01995 if (new_q[0] < bbox_min.x() || new_q[0] > bbox_max.x()) return false;
01996 if (new_q[1] < bbox_min.y() || new_q[1] > bbox_max.y()) return false;
01997 if (new_q[2] < bbox_min.z() || new_q[2] > bbox_max.z()) return false;
01998
01999 memcpy(q,new_q,7*sizeof(double));
02000
02001
02002 norm = sqrt(q[3]*q[3]+q[4]*q[4]+q[5]*q[5]+q[6]*q[6]);
02003 q[3] /= norm;
02004 q[4] /= norm;
02005 q[5] /= norm;
02006 q[6] /= norm;
02007 Quaternion rot(q[3],q[4],q[5],q[6]);
02008 vec3 cogOffset = rot * (CoG-position::ORIGIN);
02009 transf tr = transf(rot,vec3(q[0],q[1],q[2])-cogOffset);
02010
02011 Tran = tr;
02012 Tran.toSoTransform(IVTran);
02013 myWorld->getCollisionInterface()->setBodyTransform(this, Tran);
02014
02015 return true;
02016 }
02017
02022 void
02023 DynamicBody::markState()
02024 {
02025 memcpy(markedQ,q,7*sizeof(double));
02026 memcpy(markedV,v,6*sizeof(double));
02027 }
02028
02033 void
02034 DynamicBody::returnToMarkedState()
02035 {
02036 memcpy(v,markedV,6*sizeof(double));
02037 setPos(markedQ);
02038 }
02039
02043 void
02044 DynamicBody::pushState()
02045 {
02046 double *tmp = new double[7];
02047 memcpy(tmp,q,7*sizeof(double));
02048 qStack.push_back(tmp);
02049
02050 tmp = new double[6];
02051 memcpy(tmp,v,6*sizeof(double));
02052 vStack.push_back(tmp);
02053 }
02054
02058 bool
02059 DynamicBody::popState()
02060 {
02061 if (qStack.empty()) {
02062 DBGA("Pop state failed: stack empty");
02063 return false;
02064 }
02065 memcpy(v,vStack.back(),6*sizeof(double));
02066 setPos(qStack.back());
02067
02068
02069 if (++qStack.begin() != qStack.end()) {
02070 delete [] vStack.back();
02071 delete [] qStack.back();
02072 vStack.pop_back();
02073 qStack.pop_back();
02074 return true;
02075 } else {
02076 return false;
02077 }
02078 }
02079
02080 void
02081 DynamicBody::clearState()
02082 {
02083
02084
02085 if (qStack.empty()) return;
02086 while (++qStack.begin() != qStack.end()) {
02087 delete [] vStack.back();
02088 delete [] qStack.back();
02089 vStack.pop_back();
02090 qStack.pop_back();
02091 }
02092 }
02096 int
02097 DynamicBody::setTran(transf const& tr)
02098 {
02099 if (tr == Tran) return SUCCESS;
02100 if (Body::setTran(tr) == FAILURE) return FAILURE;
02101 Quaternion quat = Tran.rotation();
02102 vec3 cogOffset = quat * (CoG-position::ORIGIN);
02103 q[0] = Tran.translation().x()+cogOffset.x();
02104 q[1] = Tran.translation().y()+cogOffset.y();
02105 q[2] = Tran.translation().z()+cogOffset.z();
02106 q[3] = quat.w;
02107 q[4] = quat.x;
02108 q[5] = quat.y;
02109 q[6] = quat.z;
02110 if (fixed) {
02111 if (!dynJoint->getPrevLink()) {
02112
02113
02114
02115 fix();
02116 }
02117 }
02118 return SUCCESS;
02119 }
02120
02126 void
02127 DynamicBody::fix()
02128 {
02129 fixed = true;
02130 setDynJoint(new FixedDynJoint(NULL,this,Tran));
02131 }
02132
02137 void
02138 DynamicBody::unfix()
02139 {
02140 fixed = false;
02141 setDynJoint(NULL);
02142 }
02143
02147 void
02148 DynamicBody::setDynJoint(DynJoint *dj)
02149 {
02150 if (dynJoint) delete dynJoint;
02151 dynJoint = dj;
02152 }
02153
02155
02157
02158
02159
02160
02161
02162
02163 Link::Link(Robot *r,int c, int l,World *w,const char *name) : DynamicBody(w,name)
02164 {
02165 owner = r; chainNum = c; linkNum = l; showVC = false; showFC = false;
02166 }
02167
02168
02169
02170
02171
02172 Link::~Link()
02173 {
02174 }
02175
02176
02180 void
02181 Link::setContactsChanged()
02182 {
02183 WorldElement::setContactsChanged();
02184 owner->setContactsChanged();
02185 }
02191 bool
02192 Link::externalContactsPreventMotion(const transf& motion)
02193 {
02194 std::list<Contact *>::iterator cp;
02195 std::list<Contact *> contactList;
02196
02197 contactList = getContacts();
02198 for (cp=contactList.begin();cp!=contactList.end();cp++) {
02199 if ( (*cp)->getBody2()->getOwner() == getOwner() )
02200 continue;
02201 if ((*cp)->preventsMotion(motion)) {
02202 return true;
02203 }
02204 }
02205 return false;
02206 }
02207
02209 position
02210 Link::getDistalJointLocation()
02211 {
02212 return position(0,0,0);
02213 }
02214
02217 position
02218 Link::getProximalJointLocation()
02219 {
02220 int jointNum = owner->getChain(chainNum)->getLastJoint(linkNum);
02221 Joint *j = owner->getChain(chainNum)->getJoint(jointNum);
02222 vec3 p = j->getTran().inverse().translation();
02223 return position( p.x(), p.y(), p.z() );
02224 }
02225
02226 vec3
02227 Link::getProximalJointAxis()
02228 {
02229 int jointNum = owner->getChain(chainNum)->getLastJoint(linkNum);
02230 Joint *j = owner->getChain(chainNum)->getJoint(jointNum);
02231 vec3 r = vec3(0,0,1) * j->getTran().inverse();
02232 return r;
02233 }
02234
02236
02238
02239
02240
02241
02242 GraspableBody::GraspableBody(World *w,const char *name) : DynamicBody(w,name)
02243 {
02244 #ifdef CGDB_ENABLED
02245 mGraspitDBModel = NULL;
02246 #endif
02247 }
02248
02249
02250
02251
02252 GraspableBody::~GraspableBody()
02253 {
02254
02255 }
02256
02258 void
02259 GraspableBody::setDefaultViewingParameters()
02260 {
02261 showFC = true;
02262 showVC = true;
02263 showAxes(true);
02264 setTransparency(0.4f);
02265 }
02266
02268 void
02269 GraspableBody::cloneFrom(const GraspableBody *original)
02270 {
02271 DynamicBody::cloneFrom(original);
02272 setDefaultViewingParameters();
02273 }
02274
02278 QTextStream&
02279 operator<<(QTextStream &os, const GraspableBody &gb)
02280 {
02281 os << gb.myFilename << endl;
02282 return os;
02283 }
