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 IVScaleTran = NULL;
00118 IVOffsetTran = NULL;
00119 IVBVRoot = NULL;
00120 #ifdef GEOMETRY_LIB
00121 IVPrimitiveRoot = NULL;
00122 #endif
00123 mUsesFlock = false;
00124 mBirdNumber = 0;
00125 mRenderGeometry = true;
00126 mGeometryFilename = "none";
00127
00128 initializeIV();
00129 }
00130
00134 Body::Body(const Body &b) : WorldElement(b)
00135 {
00136 mIsElastic = b.mIsElastic;
00137 material = b.material;
00138 youngMod = b.youngMod;
00139 showFC = b.showFC;
00140 mUsesFlock = b.mUsesFlock;
00141 mRenderGeometry = true;
00142 Tran = b.Tran;
00143
00144 IVRoot = b.IVRoot;
00145 IVTran = b.IVTran;
00146 IVContactIndicators = b.IVContactIndicators;
00147 #ifdef GEOMETRY_LIB
00148 IVPrimitiveRoot = b.IVPrimitiveRoot;
00149 #endif
00150 IVBVRoot = b.IVBVRoot;
00151 IVGeomRoot = b.IVGeomRoot;
00152 IVMat = b.IVMat;
00153 mGeometryFilename = b.mGeometryFilename;
00154
00155 createAxesGeometry();
00156 }
00157
00163 Body::~Body()
00164 {
00165 breakContacts();
00166 DBGP("Deleting Body: " << myName.latin1());
00167 }
00168
00177 void
00178 Body::cloneFrom(const Body* original)
00179 {
00180 mIsElastic = original->mIsElastic;
00181 youngMod = original->youngMod;
00182 material = original->material;
00183
00184
00185 virtualContactList.clear();
00186 std::list<Contact*> vc = original->getVirtualContacts();
00187 std::list<Contact*>::iterator it;
00188 for (it = vc.begin(); it!=vc.end(); it++) {
00189 VirtualContact *newContact = new VirtualContact( (VirtualContact*)(*it) );
00190 newContact->setBody(this);
00191 addVirtualContact(newContact);
00192 }
00193
00194 setRenderGeometry( original->getRenderGeometry() );
00195
00196 int numGeomChildren = original->getIVGeomRoot()->getNumChildren();
00197
00198 for (int i=0; i<numGeomChildren; i++) {
00199 IVGeomRoot->addChild( original->getIVGeomRoot()->getChild(i) );
00200 }
00201
00202 addIVMat(true);
00203
00204
00205 cloneToIvc(original);
00206 setTran(original->getTran());
00207 }
00208
00215 int
00216 Body::loadFromXml(const TiXmlElement *root, QString rootPath)
00217 {
00218
00219 const TiXmlElement* element = findXmlElement(root,"material");
00220 QString valueStr;
00221 if(element == NULL){
00222 DBGA("No material type found; using default.");
00223 material = myWorld->getMaterialIdx("wood");
00224 } else {
00225 valueStr = element->GetText();
00226 if (!valueStr.isEmpty()) {
00227 material = myWorld->getMaterialIdx(valueStr);
00228 if (material==-1) {
00229 QTWARNING("invalid material type in body file");
00230 return FAILURE;
00231 }
00232 } else{
00233 DBGA("No material type found; using default.");
00234 material = myWorld->getMaterialIdx("wood");
00235 }
00236 }
00237
00238
00239 element = findXmlElement(root,"youngs");
00240 if(element) {
00241 valueStr = element->GetText();
00242 youngMod = valueStr.toDouble();
00243 if (youngMod <= 0) {
00244 QTWARNING("invalid Young's modulus in body file");
00245 return FAILURE;
00246 }
00247 mIsElastic = true;
00248 }
00249
00250
00251 element = findXmlElement(root,"useFlockOfBirds");
00252 if(element){
00253 valueStr = element->GetText();
00254 mBirdNumber = valueStr.toDouble();
00255 mUsesFlock = true;
00256 DBGA("Object using Flock of Birds sensor " << mBirdNumber);
00257 }
00258
00259
00260 element = findXmlElement(root,"geometryFile");
00261 if (!element) {
00262 QTWARNING("Geometry file information missing");
00263 return FAILURE;
00264 } else {
00265
00266 mGeometryFilename = element->GetText();
00267 valueStr = rootPath + mGeometryFilename;
00268 mGeometryFileType = element->Attribute("type");
00269
00270 if (mGeometryFileType.isNull()||mGeometryFileType.isEmpty()) mGeometryFileType="Inventor";
00271 int result;
00272 if (mGeometryFileType=="Inventor") {
00273 result = loadGeometryIV(valueStr);
00274 } else if (mGeometryFileType=="off") {
00275 result = loadGeometryOFF(valueStr);
00276 } else if (mGeometryFileType=="ply") {
00277 result = loadGeometryPLY(valueStr);
00278 } else {
00279 DBGA("Unknown geometry file type: " << mGeometryFileType.latin1());
00280 result = FAILURE;
00281 }
00282 if (result == FAILURE) {
00283 QTWARNING("Failed to open geometry file: " + valueStr);
00284 return FAILURE;
00285 }
00286 }
00287
00288
00289 IVScaleTran = new SoTransform;
00290 IVScaleTran->scaleFactor.setValue(1.0, 1.0, 1.0);
00291 element = findXmlElement(root,"geometryScaling");
00292 if (element) {
00293 valueStr = element->GetText();
00294 double scale = valueStr.toDouble();
00295 if (scale <= 0) {
00296 DBGA("Scale geometry: negative scale found");
00297 return FAILURE;
00298 }
00299 IVScaleTran->scaleFactor.setValue(scale, scale, scale);
00300 }
00301 IVGeomRoot->insertChild(IVScaleTran, 0);
00302
00303
00304
00305
00306 IVOffsetTran = new SoTransform;
00307 transf::IDENTITY.toSoTransform(IVOffsetTran);
00308 element = findXmlElement(root,"geometryOffset");
00309 if (element) {
00310 const TiXmlElement* transformElement = findXmlElement(element,"transform");
00311 if(!transformElement){
00312 DBGA("Geometry offset field missing transform information");
00313 return FAILURE;
00314 }
00315 transf offsetTran;
00316 if(!getTransform(transformElement, offsetTran)){
00317 DBGA("Geometry offset field: failed to parse transform");
00318 return FAILURE;
00319 }
00320 offsetTran.toSoTransform(IVOffsetTran);
00321 }
00322 IVGeomRoot->insertChild(IVOffsetTran, 0);
00323
00324 return SUCCESS;
00325 }
00326
00327 int
00328 Body::saveToXml(QTextStream& xml){
00329 xml<<"\t\t\t<material>"<<myWorld->getMaterialName(material).latin1()<<"</material>"<<endl;
00330 if(youngMod>0)
00331 xml<<"\t\t\t<youngs>"<<youngMod<<"</youngs>"<<endl;
00332 if(mUsesFlock){
00333 xml<<"\t\t\t<useFlockOfBirds>"<<mBirdNumber<<"</useFlockOfBirds>"<<endl;
00334 }
00335 xml<<"\t\t\t<geometryFile type = \""<<mGeometryFileType.latin1()<<"\">"
00336 <<mGeometryFilename.latin1()<<"</geometryFile>"<<endl;
00337 return SUCCESS;
00338 }
00339
00340
00351 int
00352 Body::load(const QString &filename)
00353 {
00354 QString fileType = filename.section('.',-1,-1);
00355 QString xmlFilename;
00356 if (fileType == "xml"){
00357
00358 xmlFilename = filename;
00359 } else {
00360
00361 DBGA("Loading geometry file with boilerplate XML file");
00362 xmlFilename = QString(getenv("GRASPIT")) + QString("/models/objects/default.xml");
00363 }
00364
00365
00366 myFilename = relativePath(filename, getenv("GRASPIT"));
00367 if (myName.isEmpty() || myName == "unnamed") {
00368 setName(filename.section('/',-1).section('.',0,0));
00369 }
00370
00371
00372 TiXmlDocument doc(xmlFilename);
00373 if(doc.LoadFile()==false){
00374 QTWARNING("Could not open " + xmlFilename);
00375 return FAILURE;
00376 }
00377 if (fileType != "xml") {
00378
00379 QString relFilename = relativePath(filename, QString(getenv("GRASPIT")) +
00380 QString("/models/objects/"));
00381 TiXmlElement * element = new TiXmlElement("geometryFile");
00382 if (fileType=="iv" || fileType=="wrl") {
00383 element->SetAttribute("type","Inventor");
00384 } else if (fileType=="off") {
00385 element->SetAttribute("type","off");
00386 } else if (fileType=="ply") {
00387 element->SetAttribute("type","ply");
00388 }
00389 TiXmlText * text = new TiXmlText( relFilename );
00390 element->LinkEndChild(text);
00391 doc.RootElement()->LinkEndChild(element);
00392 }
00393
00394 QString root = xmlFilename.section('/',0,-2,QString::SectionIncludeTrailingSep);
00395 if (loadFromXml(doc.RootElement(), root) != SUCCESS) {
00396 return FAILURE;
00397 }
00398
00399 addIVMat();
00400 return SUCCESS;
00401 }
00402
00408 int
00409 Body::loadGeometryIV(const QString &filename)
00410 {
00411 SoInput myInput;
00412 if (!myInput.openFile(filename.latin1())) {
00413 QTWARNING("Could not open Inventor file " + filename);
00414 return FAILURE;
00415 }
00416
00417
00418 SoGroup *fileGeomRoot;
00419 if (myInput.isFileVRML2()) {
00420 fileGeomRoot = SoDB::readAllVRML(&myInput);
00421 } else {
00422 fileGeomRoot = SoDB::readAll(&myInput);
00423 }
00424 myInput.closeFile();
00425 if (fileGeomRoot == NULL) {
00426 QTWARNING("A problem occurred while reading Inventor file" + filename);
00427 return FAILURE;
00428 }
00429
00430 IVGeomRoot->addChild(fileGeomRoot);
00431 return SUCCESS;
00432 }
00433
00434
00435 using std::string;
00436 using std::getline;
00437 using std::ifstream;
00438 using std::istringstream;
00439 using std::pow;
00440 using std::sqrt;
00441 using std::vector;
00442
00443
00445
00446 bool GetOffLine(ifstream* file, istringstream* line)
00447 {
00448 string buffer;
00449 if (!file->good()) return false;
00450 getline(*file, buffer);
00451
00452 buffer = buffer.substr(buffer.find_first_not_of(" \t\n\f\r"),
00453 buffer.find_first_of("#"));
00454 if (!buffer.empty()) {
00455 line->clear();
00456 line->str(buffer);
00457 return true;
00458 }
00459 return GetOffLine(file, line);
00460 }
00461
00463 int OFFReadFailure() {
00464 DBGA("OFF reader failure");
00465 return FAILURE;
00466 }
00467
00468
00474 int
00475 Body::loadGeometryOFF(const QString& filename) {
00476 ifstream file(filename.toStdString().c_str());
00477 istringstream line;
00478
00479
00480 if (!GetOffLine(&file, &line)) return OFFReadFailure();
00481
00482
00483
00484 if (!GetOffLine(&file, &line)) return OFFReadFailure();
00485 long num_vertices, num_faces;
00486 line >> num_vertices >> num_faces;
00487 if (line.fail()) return OFFReadFailure();
00488
00489 SbVec3f* vertices = new SbVec3f[num_vertices];
00490 std::vector<int32_t> face_indices;
00491
00492 for (long vertex = 0; vertex < num_vertices; ++vertex) {
00493 if (!GetOffLine(&file, &line)) return OFFReadFailure();
00494 float x, y, z;
00495 line >> x >> y >> z;
00496 if (line.fail()) return OFFReadFailure();
00497 vertices[vertex].setValue(x, y, z);
00498 }
00499
00500 for (long face = 0; face < num_faces; ++face) {
00501 if (!GetOffLine(&file, &line)) return OFFReadFailure();
00502 int num_points, vertex_index;
00503 line >> num_points;
00504
00505 for (int point = 0; point < num_points; ++point) {
00506 line >> vertex_index;
00507 face_indices.push_back(vertex_index);
00508
00509 }
00510 if (line.fail()) return OFFReadFailure();
00511 face_indices.push_back(SO_END_FACE_INDEX);
00512 }
00513
00514
00515 SoCoordinate3* coords = new SoCoordinate3;
00516 coords->point.setValues(0, num_vertices, vertices);
00517 SoIndexedFaceSet* ifs = new SoIndexedFaceSet;
00518 ifs->coordIndex.setValues(0, face_indices.size(), &(face_indices[0]));
00519 this->IVGeomRoot->addChild(coords);
00520 this->IVGeomRoot->addChild(ifs);
00521
00522 DBGA("OFF reader success");
00523 return SUCCESS;
00524 }
00525
00534 int
00535 Body::loadGeometryPLY(const QString& filename)
00536 {
00537 #ifndef PLY_READER
00538 DBGA("PLY Reader not installed; can not read file");
00539 return FAILURE;
00540 #else
00541 PLYModelLoader loader;
00542 std::vector<position> vertices;
00543 std::vector<int> triangles;
00544 if (loader.readFromFile(filename.toStdString(), vertices, triangles)) {
00545 DBGA("PLY loader error");
00546 return FAILURE;
00547 }
00548 return loadGeometryMemory(vertices, triangles);
00549 #endif
00550 }
00551
00552 int
00553 Body::loadGeometryMemory(const std::vector<position> &vertices, const std::vector<int> &triangles)
00554 {
00555 int num_vertices = vertices.size();
00556 SbVec3f* sbVertices = new SbVec3f[num_vertices];
00557 for (size_t i=0; i<vertices.size(); i++) {
00558 sbVertices[i].setValue(vertices[i].x(), vertices[i].y(), vertices[i].z());
00559 }
00560 SoCoordinate3* coords = new SoCoordinate3;
00561 coords->point.setValues(0, num_vertices, sbVertices);
00562 std::vector<int32_t> face_indices;
00563 for (size_t i=0; i<triangles.size(); i++) {
00564 face_indices.push_back(triangles[i]);
00565 if (i%3==2) {
00566 face_indices.push_back(SO_END_FACE_INDEX);
00567 }
00568 }
00569 SoIndexedFaceSet* ifs = new SoIndexedFaceSet;
00570 ifs->coordIndex.setValues(0, face_indices.size(), &(face_indices[0]));
00571 this->IVGeomRoot->addChild(coords);
00572 this->IVGeomRoot->addChild(ifs);
00573 return SUCCESS;
00574 }
00575
00583 void
00584 Body::addIVMat(bool clone)
00585 {
00586 IVMat = new SoMaterial;
00587 IVMat->diffuseColor.setIgnored(true);
00588 IVMat->ambientColor.setIgnored(true);
00589 IVMat->specularColor.setIgnored(true);
00590 IVMat->emissiveColor.setIgnored(true);
00591 IVMat->shininess.setIgnored(true);
00592
00593 if (clone) {
00594
00595 IVGeomRoot->addChild(IVMat);
00596 } else {
00597 SoSearchAction *sa = new SoSearchAction;
00598 sa->setInterest(SoSearchAction::ALL);
00599 sa->setType(SoMaterial::getClassTypeId());
00600 sa->apply(IVGeomRoot);
00601
00602 if (sa->getPaths().getLength() == 0) {
00603 IVGeomRoot->insertChild(IVMat,0);
00604 } else {
00605 for (int i=0; i<sa->getPaths().getLength(); i++) {
00606 SoGroup *g = (SoGroup *)sa->getPaths()[i]->getNodeFromTail(1);
00607 if (((SoMaterial *)sa->getPaths()[i]->getTail())->transparency[0] == 0.0f) {
00608 g->insertChild(IVMat,sa->getPaths()[i]->getIndexFromTail(0)+1);
00609 }
00610 }
00611 }
00612 delete sa;
00613 }
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624 }
00625
00628 void
00629 Body::addToIvc(bool ExpectEmpty)
00630 {
00631 myWorld->getCollisionInterface()->addBody(this, ExpectEmpty);
00632 myWorld->getCollisionInterface()->setBodyTransform(this, Tran);
00633 }
00634
00639 void
00640 Body::cloneToIvc(const Body *original)
00641 {
00642 myWorld->getCollisionInterface()->cloneBody(this, original);
00643 myWorld->getCollisionInterface()->setBodyTransform(this, Tran);
00644 }
00645
00648 void Body::setGeometryScaling(double x, double y, double z)
00649 {
00650 if (x<=0 || y<=0 || z<=0)
00651 {
00652 DBGA("Scale geometry: negative or zero scale found");
00653 return;
00654 }
00655 IVScaleTran->scaleFactor.setValue(x, y, z);
00656 myWorld->getCollisionInterface()->updateBodyGeometry(this);
00657 }
00658
00661 void Body::setGeometryOffset(transf tr)
00662 {
00663 tr.toSoTransform(IVOffsetTran);
00664 myWorld->getCollisionInterface()->updateBodyGeometry(this);
00665 }
00666
00667 void
00668 Body::setDefaultViewingParameters()
00669 {
00670 showFC = false;
00671 showVC = false;
00672 setTransparency(0.0);
00673 }
00674
00678 void
00679 Body::initializeIV()
00680 {
00681 IVRoot = new SoSeparator;
00682 IVTran = new SoTransform;
00683 IVRoot->insertChild(IVTran,0);
00684
00685
00686
00687 createAxesGeometry();
00688
00689 IVContactIndicators = new SoSeparator;
00690 IVRoot->addChild(IVContactIndicators);
00691
00692 #ifdef GEOMETRY_LIB
00693 IVPrimitiveRoot = new SoSeparator;
00694 IVRoot->addChild(IVPrimitiveRoot);
00695 #endif
00696
00697 IVBVRoot = new SoSeparator;
00698 IVRoot->addChild(IVBVRoot);
00699
00700 IVGeomRoot = new SoSeparator;
00701 IVRoot->addChild(IVGeomRoot);
00702 }
00703
00707 void
00708 Body::setBVGeometry(const std::vector<BoundingBox> &bvs)
00709 {
00710 IVBVRoot->removeAllChildren();
00711 int mark = 0;
00712 for (int i=0; i<(int)bvs.size(); i++) {
00713 SoSeparator *bvSep = new SoSeparator;
00714
00715 SoMaterial *bvMat = new SoMaterial;
00716 bvSep->addChild(bvMat);
00717 float r,g,b;
00718
00719 r = ((float)rand()) / RAND_MAX;
00720 g = ((float)rand()) / RAND_MAX;
00721 b = ((float)rand()) / RAND_MAX;
00722
00723
00724 if (bvs[i].mMark) {
00725 mark++;
00726 r = 0.8f; g=0.0f; b=0.0f;
00727 } else {
00728 r = g = b = 0.5f;
00729 }
00730
00731 bvMat->diffuseColor = SbColor(r,g,b);
00732 bvMat->ambientColor = SbColor(r,g,b);
00733 bvMat->transparency = 0.5;
00734
00735 SoTransform* bvTran = new SoTransform;
00736 bvs[i].getTran().toSoTransform(bvTran);
00737 bvSep->addChild(bvTran);
00738
00739
00740
00741 SoCube *bvBox = new SoCube;
00742 bvBox->width = 2 * bvs[i].halfSize.x();
00743 bvBox->height = 2 * bvs[i].halfSize.y();
00744 bvBox->depth = 2 * bvs[i].halfSize.z();
00745 bvSep->addChild(bvBox);
00746
00747
00748
00749
00750
00751
00752
00753
00754
00755
00756
00757
00758
00759
00760
00761
00762
00763
00764 IVBVRoot->addChild(bvSep);
00765 }
00766 DBGA("Setting bv geom: " << bvs.size() << " boxes. Marked: " << mark);
00767 }
00768
00773 float
00774 Body::getTransparency() const
00775 {
00776 return IVMat->transparency[0];
00777 }
00778
00779
00785 void
00786 Body::setTransparency(float t)
00787 {
00788 IVMat->transparency = t;
00789 }
00790
00791
00796 void
00797 Body::setMaterial(int mat)
00798 {
00799 std::list<Contact *>::iterator cp;
00800
00801 material = mat;
00802 if (showFC || showVC) IVContactIndicators->removeAllChildren();
00803
00804 for (cp=contactList.begin();cp!=contactList.end();cp++) {
00805 (*cp)->updateCof();
00806 (*cp)->getMate()->updateCof();
00807 (*cp)->getBody2()->redrawFrictionCones();
00808 }
00809 redrawFrictionCones();
00810 }
00811
00812
00816 void
00817 Body::showFrictionCones(bool on, int vc)
00818 {
00819 showFC = on;
00820 if (vc==1) showVC = true;
00821 else if (vc==2) showVC = false;
00822 redrawFrictionCones();
00823 }
00824
00825
00829 void
00830 Body::redrawFrictionCones()
00831 {
00832 std::list<Contact *>::iterator cp;
00833
00834 IVContactIndicators->removeAllChildren();
00835 if (showFC) {
00836 for (cp=contactList.begin();cp!=contactList.end();cp++)
00837 IVContactIndicators->addChild( (*cp)->getVisualIndicator() );
00838 }
00839 if (showVC) {
00840 for (cp = virtualContactList.begin(); cp!=virtualContactList.end(); cp++)
00841 IVContactIndicators->addChild( ( (VirtualContact*)(*cp) )->getVisualIndicator() );
00842 }
00843 }
00844
00848 void
00849 Body::setRenderGeometry(bool s)
00850 {
00851 assert(IVTran);
00852 mRenderGeometry = s;
00853 if (!s) {
00854
00855
00856
00857
00858
00859 IVTran->translation.enableNotify(false);
00860 IVTran->rotation.enableNotify(false);
00861 } else {
00862
00863
00864
00865
00866
00867 IVTran->translation.enableNotify(true);
00868 IVTran->rotation.enableNotify(true);
00869 }
00870
00871 }
00872
00877 int
00878 Body::setTran(transf const &tr)
00879 {
00880 if (tr == Tran) return SUCCESS;
00881 breakContacts();
00882
00883 if (!myWorld->wasModified() && tr != Tran) {
00884 myWorld->setModified();
00885 }
00886 Tran = tr;
00887 myWorld->getCollisionInterface()->setBodyTransform(this, Tran);
00888 if (IVTran) {
00889 Tran.toSoTransform(IVTran);
00890 }
00891 return SUCCESS;
00892 }
00893
00894
00899 bool
00900 Body::contactsPreventMotion(const transf& motion) const
00901 {
00902 std::list<Contact *>::iterator cp;
00903 std::list<Contact *> contactList;
00904
00905 contactList = getContacts();
00906 for (cp=contactList.begin();cp!=contactList.end();cp++) {
00907 if ((*cp)->preventsMotion(motion)) {
00908 return true;
00909 }
00910 }
00911 return false;
00912 }
00913
00919 void
00920 Body::breakContacts()
00921 {
00922 std::list<Contact *>::iterator cp;
00923
00924 if (!contactList.empty()) {
00925 setContactsChanged();
00926 for (cp=contactList.begin();cp!=contactList.end();cp++) {
00927 delete *cp; *cp = NULL;
00928 }
00929 contactList.clear();
00930 }
00931 numContacts = 0;
00932
00933 for (cp = prevContactList.begin(); cp!=prevContactList.end(); cp++) {
00934 if ( (*cp)->getMate() != NULL ) {
00935 (*cp)->getMate()->setMate(NULL);
00936 }
00937 (*cp)->setMate(NULL);
00938 delete *cp; *cp = NULL;
00939 }
00940 prevContactList.clear();
00941
00942 if (showFC)
00943 IVContactIndicators->removeAllChildren();
00944 }
00945
00947 int
00948 Body::loadContactData(QString fn)
00949 {
00950 FILE *fp = fopen(fn.latin1(), "r");
00951 if (!fp) {
00952 fprintf(stderr,"Could not open filename %s\n",fn.latin1());
00953 return FAILURE;
00954 }
00955 int numContacts;
00956 VirtualContactOnObject *newContact;
00957 if(fscanf(fp,"%d",&numContacts) <= 0)
00958 return FAILURE;
00959
00960 breakVirtualContacts();
00961 for (int i=0; i<numContacts; i++) {
00962 newContact = new VirtualContactOnObject();
00963 newContact->readFromFile(fp);
00964 newContact->setBody( this );
00965 ((Contact*)newContact)->computeWrenches();
00966 addVirtualContact( newContact );
00967 }
00968 fclose(fp);
00969 return SUCCESS;
00970 }
00973 void
00974 Body::breakVirtualContacts()
00975 {
00976 std::list<Contact *>::iterator cp;
00977 for (cp = virtualContactList.begin(); cp!= virtualContactList.end(); cp++) {
00978 delete *cp;
00979 }
00980
00981 if (showVC) {
00982 IVContactIndicators->removeAllChildren();
00983 }
00984 virtualContactList.clear();
00985 }
00986
00991 void
00992 Body::resetContactList()
00993 {
00994 std::list<Contact *>::iterator cp;
00995 for (cp = prevContactList.begin(); cp!=prevContactList.end(); cp++) {
00996 if ( (*cp)->getMate() != NULL ) {
00997 (*cp)->getMate()->setMate(NULL);
00998 }
00999 (*cp)->setMate(NULL);
01000 delete *cp; *cp = NULL;
01001 }
01002 prevContactList.clear();
01003 if (!contactList.empty()) {
01004 setContactsChanged();
01005 for (cp=contactList.begin();cp!=contactList.end();cp++) {
01006 prevContactList.push_back(*cp);
01007 }
01008 contactList.clear();
01009 }
01010 numContacts = 0;
01011 if (showFC)
01012 IVContactIndicators->removeAllChildren();
01013 }
01014
01020 void
01021 Body::addContact(Contact *c)
01022 {
01023 setContactsChanged();
01024 contactList.push_back(c);
01025 numContacts++;
01026 if (showFC) {
01027 assert(IVContactIndicators->getNumChildren() > numContacts-2);
01028 IVContactIndicators->insertChild( c->getVisualIndicator(), numContacts-1 );
01029 }
01030 }
01031
01036 int
01037 Body::getNumContacts(Body *b) const
01038 {
01039 if (!b) return numContacts;
01040 int c = 0;
01041 std::list<Contact*>::const_iterator it;
01042 for (it = contactList.begin(); it!=contactList.end(); it++) {
01043 if ( (*it)->getBody2() == b) c++;
01044 }
01045 return c;
01046 }
01047
01052 std::list<Contact*>
01053 Body::getContacts(Body *b) const
01054 {
01055 if (!b) return contactList;
01056 std::list<Contact*> contacts;
01057 std::list<Contact*>::const_iterator it;
01058 for (it = contactList.begin(); it!=contactList.end(); it++) {
01059 if ( (*it)->getBody2() == b) contacts.push_back(*it);
01060 }
01061 return contacts;
01062 }
01063
01065 void
01066 Body::addVirtualContact(Contact *c)
01067 {
01068 setContactsChanged();
01069 virtualContactList.push_back(c);
01070 if (showVC)
01071 IVContactIndicators->addChild( c->getVisualIndicator() );
01072 }
01073
01080 Contact*
01081 Body::checkContactInheritance(Contact *c)
01082 {
01083 std::list<Contact *>::iterator cp;
01084 bool inheritance = false;
01085 for (cp = prevContactList.begin(); cp != prevContactList.end(); cp++) {
01086 if ( (*cp)->getBody1() != c->getBody1() )
01087 continue;
01088 if ( (*cp)->getBody2() != c->getBody2() )
01089 continue;
01090 vec3 d = (*cp)->getPosition() - c->getPosition();
01091 if (d.len() > Contact::INHERITANCE_THRESHOLD )
01092 continue;
01093 vec3 n1 = (*cp)->getNormal();
01094 vec3 n2 = c->getNormal();
01095 double theta = n1 % n2;
01096 if ( theta < Contact::INHERITANCE_ANGULAR_THRESHOLD )
01097 continue;
01098 inheritance = true;
01099 break;
01100 }
01101 if (!inheritance)
01102 return NULL;
01103 return (*cp);
01104 }
01105
01106
01111 void
01112 Body::removeContact(Contact *c)
01113 {
01114 int i;
01115 std::list<Contact *>::iterator cp;
01116
01117 setContactsChanged();
01118
01119 if (showFC) {
01120 for (cp=contactList.begin(),i=0;cp!=contactList.end();cp++,i++)
01121 if (*cp == c) {
01122 contactList.erase(cp);
01123 break;
01124 }
01125 IVContactIndicators->removeChild(i);
01126 }
01127 else contactList.remove(c);
01128
01129 delete c;
01130 numContacts--;
01131 }
01132
01136 void Body::removePrevContact(Contact *c)
01137 {
01138 prevContactList.remove(c);
01139 c->setMate(NULL);
01140 delete c;
01141 }
01142
01146 QTextStream&
01147 operator<<(QTextStream &os, const Body &b)
01148 {
01149 os << b.myFilename << endl;
01150 os << b.myWorld->getMaterialName(b.material) << endl;
01151 return os;
01152 }
01153
01154
01156 void addTriangleCallBack(void* info, SoCallbackAction * action,
01157 const SoPrimitiveVertex * v1,
01158 const SoPrimitiveVertex * v2,
01159 const SoPrimitiveVertex * v3)
01160 {
01161 std::vector<Triangle> *triangles = (std::vector<Triangle>*) info;
01162
01163 SbVec3f p1, p2, p3;
01164 SbMatrix mm = action->getModelMatrix();
01165
01166
01167 mm.multVecMatrix( v1->getPoint(), p1 );
01168 mm.multVecMatrix( v2->getPoint(), p2 );
01169 mm.multVecMatrix( v3->getPoint(), p3 );
01170
01171
01172 if ((p1 == p2) || (p2 == p3) || (p1 == p3)) return;
01173
01174 position nv1(p1[0], p1[1], p1[2]);
01175 position nv2(p2[0], p2[1], p2[2]);
01176 position nv3(p3[0], p3[1], p3[2]);
01177 Triangle newTri(nv1,nv2,nv3);
01178 triangles->push_back( newTri );
01179 }
01180
01182 void addVertexCallBack(void* info, SoCallbackAction * action, const SoPrimitiveVertex * v1)
01183 {
01184 std::vector<position> *vertices = (std::vector<position>*) info;
01185 SbVec3f p1;
01186 SbMatrix mm = action->getModelMatrix();
01187
01188 mm.multVecMatrix( v1->getPoint(), p1 );
01189 position nv1(p1[0], p1[1], p1[2]);
01190 vertices->push_back( nv1 );
01191 }
01192
01194 void addVerticesFromTriangleCallBack(void* info, SoCallbackAction * action,
01195 const SoPrimitiveVertex * v1,
01196 const SoPrimitiveVertex * v2,
01197 const SoPrimitiveVertex * v3)
01198 {
01199 std::vector<position> *vertices = (std::vector<position>*) info;
01200 SbVec3f p1, p2, p3;
01201 SbMatrix mm = action->getModelMatrix();
01202
01203 mm.multVecMatrix( v1->getPoint(), p1 );
01204 mm.multVecMatrix( v2->getPoint(), p2 );
01205 mm.multVecMatrix( v3->getPoint(), p3 );
01206 vertices->push_back(position(p1[0], p1[1], p1[2]));
01207 vertices->push_back(position(p2[0], p2[1], p2[2]));
01208 vertices->push_back(position(p3[0], p3[1], p3[2]));
01209 }
01210
01212 void
01213 Body::getGeometryTriangles(std::vector<Triangle> *triangles) const
01214 {
01215 SoCallbackAction ca;
01216 ca.addTriangleCallback(SoShape::getClassTypeId(), addTriangleCallBack, triangles);
01217 ca.apply(getIVGeomRoot());
01218 }
01219
01224 void
01225 Body::getGeometryVertices(std::vector<position> *vertices) const
01226 {
01227 SoCallbackAction ca;
01228
01229
01230
01231 ca.addTriangleCallback(SoShape::getClassTypeId(), addVerticesFromTriangleCallBack, vertices);
01232 ca.apply(getIVGeomRoot());
01233 }
01234
01238 void Body::createAxesGeometry()
01239 {
01240 IVWorstCase = new SoSeparator;
01241 IVAxes = new SoSwitch;
01242 if (graspItGUI) {
01243 SoSeparator *axesSep = new SoSeparator;
01244 axesTranToCOG = new SoTranslation;
01245 axesTranToCOG->translation.setValue(0,0,0);
01246 axesSep->addChild(axesTranToCOG);
01247 axesSep->addChild(IVWorstCase);
01248
01249 axesScale = new SoScale;
01250 axesScale->scaleFactor = SbVec3f(1,1,1);
01251 axesSep->addChild(axesScale);
01252 axesSep->addChild(graspItGUI->getIVmgr()->getPointers()->getChild(2));
01253 IVAxes->addChild(axesSep);
01254 }
01255 if (!IVRoot) IVRoot = new SoSeparator;
01256
01257 IVRoot->addChild(IVAxes);
01258 }
01259
01261
01263
01268 DynamicBody::~DynamicBody()
01269 {
01270 if (dynJoint) delete dynJoint;
01271 }
01272
01273 void
01274 DynamicBody::init()
01275 {
01276 maxRadius = 0.0; mass = 0.0;
01277 showAx = showDynCF = false;
01278 fixed = false; dynJoint=NULL; dynamicsComputedFlag = false;
01279 useDynamics = true;
01280 bbox_min = vec3(-1e+6,-1e+6,-1e+6);
01281 bbox_max = vec3(1e+6,1e+6,1e+6);
01282 CoG.set(0.0,0.0,0.0);
01283 for(int i=0; i<9; i++) {
01284 I[i] = 0.0;
01285 }
01286 resetDynamics();
01287 }
01288
01293 void
01294 DynamicBody::resetDynamics()
01295 {
01296 resetExtWrenchAcc();
01297 for (int i=0; i<6; i++) {
01298 a[i] = 0.0;
01299 v[i] = 0.0;
01300 }
01301 Quaternion quat = Tran.rotation();
01302 vec3 cogOffset = quat * (CoG-position::ORIGIN);
01303 q[0] = Tran.translation().x()+cogOffset.x();
01304 q[1] = Tran.translation().y()+cogOffset.y();
01305 q[2] = Tran.translation().z()+cogOffset.z();
01306 q[3] = quat.w;
01307 q[4] = quat.x;
01308 q[5] = quat.y;
01309 q[6] = quat.z;
01310 }
01311
01316 DynamicBody::DynamicBody(World *w, const char *name) : Body(w,name)
01317 {
01318 init();
01319 }
01320
01326 DynamicBody::DynamicBody(const Body &b, double m) : Body(b)
01327 {
01328 init();
01329 position defCoG;
01330 double defI[9];
01331 computeDefaultMassProp(defCoG, defI);
01332 setMass(m);
01333 setCoG(defCoG);
01334 setInertiaMatrix(defI);
01335 setMaxRadius(computeDefaultMaxRadius());
01336
01337 setTran(b.getTran());
01338 }
01339
01343 void
01344 DynamicBody::cloneFrom(const DynamicBody *original)
01345 {
01346 Body::cloneFrom(original);
01347 setMass(original->mass);
01348 setCoG(original->CoG);
01349 setInertiaMatrix(original->I);
01350 setMaxRadius(original->maxRadius);
01351 }
01352
01353 int
01354 DynamicBody::loadFromXml(const TiXmlElement *root, QString rootPath)
01355 {
01356 if (Body::loadFromXml(root, rootPath)==FAILURE) {
01357 return FAILURE;
01358 }
01359 double loadMass;
01360 bool overrideI, overrideCog;
01361 QString valueStr;
01362
01363 const TiXmlElement* element = findXmlElement(root, "mass");
01364 if(element == NULL){
01365 loadMass = defaultMass;
01366 DBGA("Using default mass");
01367 } else{
01368 valueStr = element->GetText();
01369 loadMass = valueStr.toDouble();
01370 if (loadMass <= 0) {
01371 QTWARNING("invalid mass in dynamic body file: " + myFilename);
01372 return FAILURE;
01373 }
01374 }
01375 element = findXmlElement(root, "cog");
01376 position loadCog;
01377 if(element == NULL){
01378 overrideCog = false;
01379 } else{
01380 overrideCog = true;
01381 valueStr = element->GetText();
01382 valueStr = valueStr.simplifyWhiteSpace();
01383 QStringList l;
01384 l = QStringList::split(' ', valueStr.stripWhiteSpace());
01385 if(l.count()!=3){
01386 QTWARNING("Invalid Center of Gravity Input");
01387 return FAILURE;
01388 }
01389 double x,y,z;
01390 x = l[0].toDouble();
01391 y = l[1].toDouble();
01392 z = l[2].toDouble();
01393 loadCog = position(x,y,z);
01394 }
01395 double loadI[9];
01396 element = findXmlElement(root, "inertia_matrix");
01397 if(element == NULL){
01398 overrideI = false;
01399 } else{
01400 overrideI = true;
01401 valueStr = element->GetText();
01402 valueStr = valueStr.simplifyWhiteSpace();
01403 QStringList l;
01404 l = QStringList::split(' ', valueStr.stripWhiteSpace());
01405 if(l.count()!=9){
01406 QTWARNING("Invalid Inertia Matrix Input");
01407 return FAILURE;
01408 }
01409 loadI[0]=l[0].toDouble();
01410 loadI[1]=l[1].toDouble();
01411 loadI[2]=l[2].toDouble();
01412 loadI[3]=l[3].toDouble();
01413 loadI[4]=l[4].toDouble();
01414 loadI[5]=l[5].toDouble();
01415 loadI[6]=l[6].toDouble();
01416 loadI[7]=l[7].toDouble();
01417 loadI[8]=l[8].toDouble();
01418 }
01419 if (!overrideI || !overrideCog) {
01420 position defaultCog;
01421 double defaultI[9];
01422 computeDefaultMassProp(defaultCog, defaultI);
01423 if (!overrideI) {
01424 memcpy(loadI, defaultI, 9*sizeof(double));
01425 DBGA("Using default inertia matrix");
01426 }
01427 if (!overrideCog) {
01428 loadCog = defaultCog;
01429 DBGA("Using default center of gravity");
01430 }
01431 }
01432 setMass(loadMass);
01433 setCoG(loadCog);
01434 setInertiaMatrix(loadI);
01435 setMaxRadius(computeDefaultMaxRadius());
01436 return SUCCESS;
01437 }
01438
01443 void
01444 DynamicBody::setDefaultDynamicParameters()
01445 {
01446 position defaultCog;
01447 double defaultI[9];
01448 computeDefaultMassProp(defaultCog, defaultI);
01449 setCoG(defaultCog);
01450 setInertiaMatrix(defaultI);
01451 }
01452
01453 int
01454 DynamicBody::saveToXml(QTextStream &xml){
01455 if(Body::saveToXml(xml)!=SUCCESS) return FAILURE;
01456 xml<<"\t\t\t<mass>"<<mass<<"</mass>"<<endl;
01457 xml<<"\t\t\t<cog>"<<CoG.x()<<" "<<CoG.y()<<" "<<CoG.z()<<"</cog>"<<endl;
01458 xml<<"\t\t\t<inertia_matrix>";
01459 for(int i=0;i<9;i++){
01460 xml<<I[i];
01461 if(i!=8)
01462 xml<<" ";
01463 }
01464 xml<<"</inertia_matrix>"<<endl;
01465 return SUCCESS;
01466 }
01470 void
01471 DynamicBody::setCoG(const position &newCoG)
01472 {
01473 CoG = newCoG;
01474 resetDynamics();
01475
01476
01477
01478
01479 axesTranToCOG->translation.setValue(0,0,0);
01480 }
01481
01485 void
01486 DynamicBody::setMaxRadius(double maxRad)
01487 {
01488 maxRadius = maxRad;
01489 axesScale->scaleFactor = SbVec3f(maxRadius / AXES_SCALE, maxRadius / AXES_SCALE, maxRadius / AXES_SCALE);
01490 }
01491
01492 void
01493 DynamicBody::setInertiaMatrix(const double *newI)
01494 {
01495 memcpy(I, newI, 9*sizeof(double));
01496 }
01497
01498 double
01499 DynamicBody::computeDefaultMaxRadius()
01500 {
01501 std::vector<position> vertices;
01502 getGeometryVertices(&vertices);
01503 if (vertices.empty()) {
01504 DBGA("No vertices found when computing maxRadius!");
01505 }
01506 double maxRad = 0.0;
01507 for (int i=0; i<(int)vertices.size(); i++) {
01508 double tmpRadius = (CoG - vertices[i]).len();
01509 if (tmpRadius > maxRad) maxRad = tmpRadius;
01510 }
01511 return maxRad;
01512 }
01513
01518 void
01519 DynamicBody::compProjectionIntegrals(FACE &f, int A, int B)
01520 {
01521 double a0, a1, da;
01522 double b0, b1, db;
01523 double a0_2, a0_3, a0_4, b0_2, b0_3, b0_4;
01524 double a1_2, a1_3, b1_2, b1_3;
01525 double C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb;
01526 double Cab, Kab, Caab, Kaab, Cabb, Kabb;
01527 int i;
01528
01529 P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0;
01530
01531 for (i = 0; i < 3; i++) {
01532 a0 = f.verts[i][A];
01533 b0 = f.verts[i][B];
01534 a1 = f.verts[(i+1) % 3][A];
01535 b1 = f.verts[(i+1) % 3][B];
01536 da = a1 - a0;
01537 db = b1 - b0;
01538 a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0;
01539 b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0;
01540 a1_2 = a1 * a1; a1_3 = a1_2 * a1;
01541 b1_2 = b1 * b1; b1_3 = b1_2 * b1;
01542
01543 C1 = a1 + a0;
01544 Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4;
01545 Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4;
01546 Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2;
01547 Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3;
01548 Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3;
01549 Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3;
01550
01551 P1 += db*C1;
01552 Pa += db*Ca;
01553 Paa += db*Caa;
01554 Paaa += db*Caaa;
01555 Pb += da*Cb;
01556 Pbb += da*Cbb;
01557 Pbbb += da*Cbbb;
01558 Pab += db*(b1*Cab + b0*Kab);
01559 Paab += db*(b1*Caab + b0*Kaab);
01560 Pabb += da*(a1*Cabb + a0*Kabb);
01561 }
01562
01563 P1 /= 2.0;
01564 Pa /= 6.0;
01565 Paa /= 12.0;
01566 Paaa /= 20.0;
01567 Pb /= -6.0;
01568 Pbb /= -12.0;
01569 Pbbb /= -20.0;
01570 Pab /= 24.0;
01571 Paab /= 60.0;
01572 Pabb /= -60.0;
01573 }
01574
01579 void
01580 DynamicBody::compFaceIntegrals(FACE &f,int A, int B, int C)
01581 {
01582 double *n, w;
01583 double k1, k2, k3, k4;
01584
01585 compProjectionIntegrals(f,A,B);
01586
01587 w = f.w;
01588 n = f.norm;
01589 k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1;
01590
01591 Fa = k1 * Pa;
01592 Fb = k1 * Pb;
01593 Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1);
01594
01595 Faa = k1 * Paa;
01596 Fbb = k1 * Pbb;
01597 Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb
01598 + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1));
01599
01600 Faaa = k1 * Paaa;
01601 Fbbb = k1 * Pbbb;
01602 Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab
01603 + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb
01604 + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb)
01605 + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1));
01606
01607 Faab = k1 * Paab;
01608 Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb);
01609 Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb
01610 + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa));
01611 }
01612
01617 int
01618 DynamicBody::computeDefaultInertiaMatrix(std::vector<Triangle> &triangles, double *defaultI)
01619 {
01620 FACE f;
01621 double dx1,dy1,dz1,dx2,dy2,dz2;
01622 double nx, ny, nz,len;
01623 int i,A,B,C;
01624 double r[3], v1[3], v2[3], v3[3];
01625 double density;
01626
01627 f.verts[0] = v1;
01628 f.verts[1] = v2;
01629 f.verts[2] = v3;
01630
01631
01632 double T0, T1[3], T2[3], TP[3];
01633
01634 T0 = T1[0] = T1[1] = T1[2]
01635 = T2[0] = T2[1] = T2[2]
01636 = TP[0] = TP[1] = TP[2] = 0;
01637
01638
01639 for (i = 0; i < (int)triangles.size(); i++) {
01640 triangles[i].v1.get(v1);
01641 triangles[i].v2.get(v2);
01642 triangles[i].v3.get(v3);
01643
01644 dx1 = f.verts[1][0] - f.verts[0][0];
01645 dy1 = f.verts[1][1] - f.verts[0][1];
01646 dz1 = f.verts[1][2] - f.verts[0][2];
01647 dx2 = f.verts[2][0] - f.verts[1][0];
01648 dy2 = f.verts[2][1] - f.verts[1][1];
01649 dz2 = f.verts[2][2] - f.verts[1][2];
01650 nx = dy1 * dz2 - dy2 * dz1;
01651 ny = dz1 * dx2 - dz2 * dx1;
01652 nz = dx1 * dy2 - dx2 * dy1;
01653 len = sqrt(nx * nx + ny * ny + nz * nz);
01654
01655 f.norm[0] = nx / len;
01656 f.norm[1] = ny / len;
01657 f.norm[2] = nz / len;
01658 f.w = - f.norm[0] * f.verts[0][0]
01659 - f.norm[1] * f.verts[0][1]
01660 - f.norm[2] * f.verts[0][2];
01661
01662 nx = fabs(f.norm[0]);
01663 ny = fabs(f.norm[1]);
01664 nz = fabs(f.norm[2]);
01665 if (nx > ny && nx > nz) C = 0;
01666 else C = (ny > nz) ? 1 : 2;
01667 A = (C + 1) % 3;
01668 B = (A + 1) % 3;
01669
01670 compFaceIntegrals(f,A,B,C);
01671
01672 T0 += f.norm[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc));
01673
01674 T1[A] += f.norm[A] * Faa;
01675 T1[B] += f.norm[B] * Fbb;
01676 T1[C] += f.norm[C] * Fcc;
01677 T2[A] += f.norm[A] * Faaa;
01678 T2[B] += f.norm[B] * Fbbb;
01679 T2[C] += f.norm[C] * Fccc;
01680 TP[A] += f.norm[A] * Faab;
01681 TP[B] += f.norm[B] * Fbbc;
01682 TP[C] += f.norm[C] * Fcca;
01683 }
01684
01685 T1[0] /= 2; T1[1] /= 2; T1[2] /= 2;
01686 T2[0] /= 3; T2[1] /= 3; T2[2] /= 3;
01687 TP[0] /= 2; TP[1] /= 2; TP[2] /= 2;
01688
01689 r[0] = T1[0] / T0;
01690 r[1] = T1[1] / T0;
01691 r[2] = T1[2] / T0;
01692
01693 DBGP("COM: " << r[0] << " " << r[1] << " " << r[2]);
01694
01695
01696 for (int i=0; i<9; i++) {
01697 defaultI[i] = 0.0;
01698 }
01699 defaultI[0] = defaultI[3] = defaultI[6] = 1.0;
01700
01701
01702 if (r[0] != r[0]) return FAILURE;
01703 if (r[1] != r[1]) return FAILURE;
01704 if (r[2] != r[2]) return FAILURE;
01705 if (fabs(T0) < 1.0e-5) return FAILURE;
01706 if ( fabs(r[0]) > 5.0e2 || fabs(r[1]) > 5.0e2 || fabs(r[2]) > 5.0e2 ) {
01707 return FAILURE;
01708 }
01709
01710
01711 density = 1.0 / T0;
01712
01713
01714 defaultI[0] = density * (T2[1] + T2[2]);
01715 defaultI[4] = density * (T2[2] + T2[0]);
01716 defaultI[8] = density * (T2[0] + T2[1]);
01717 defaultI[1] = defaultI[3] = - density * TP[0];
01718 defaultI[5] = defaultI[7] = - density * TP[1];
01719 defaultI[6] = defaultI[2] = - density * TP[2];
01720
01721
01722 defaultI[0] -= (r[1]*r[1] + r[2]*r[2]);
01723 defaultI[4] -= (r[2]*r[2] + r[0]*r[0]);
01724 defaultI[8] -= (r[0]*r[0] + r[1]*r[1]);
01725 defaultI[1] = defaultI[3] += r[0] * r[1];
01726 defaultI[5] = defaultI[7] += r[1] * r[2];
01727 defaultI[6] = defaultI[2] += r[2] * r[0];
01728 return SUCCESS;
01729 }
01730
01735 template <class IntegrableFunction>
01736 float Gaussian7Integrate(const Triangle& triangle, IntegrableFunction integrable_function) {
01737
01738 static const float terms[8] = {
01739 0.333333333333333333333333333333333f,
01740 0.79742698535308732239802527616971f,
01741 0.10128650732345633880098736191512f,
01742 0.059715871789769820459117580973143f,
01743 0.47014206410511508977044120951345f,
01744 0.225f,
01745 0.12593918054482715259568394550018f,
01746 0.13239415278850618073764938783315f};
01747
01748 static const float barycentric_weights[7][3] = {
01749 {terms[0], terms[0], terms[0]},
01750 {terms[1], terms[2], terms[2]},
01751 {terms[2], terms[1], terms[2]},
01752 {terms[2], terms[2], terms[1]},
01753 {terms[3], terms[4], terms[4]},
01754 {terms[4], terms[3], terms[4]},
01755 {terms[4], terms[4], terms[3]}};
01756 static const float sample_weights[7] = {
01757 terms[5], terms[6], terms[6], terms[6],
01758 terms[7], terms[7], terms[7]};
01759
01760 float integration_samples[7][3] = {{0}};
01761 const position* vertices[3] = {&(triangle.v1), &(triangle.v2), &(triangle.v3)};
01762 for(int sample_num = 0; sample_num < 7; ++sample_num) {
01763 float* sample = integration_samples[sample_num];
01764 const float* barycentric_weight = barycentric_weights[sample_num];
01765 for (int vertex_num = 0; vertex_num < 3; ++vertex_num) {
01766 const position& vertex = *(vertices[vertex_num]);
01767 for (int coord = 0; coord < 3; ++coord) {
01768 sample[coord] += vertex[coord] * barycentric_weight[vertex_num];
01769 }
01770 }
01771 }
01772
01773 float result = 0;
01774 for (int sample = 0; sample < 7; ++sample){
01775 result += integrable_function(integration_samples[sample]) * sample_weights[sample];
01776 }
01777 return result * triangle.area();
01778 }
01779
01781 struct GetCoord {
01782 int coord;
01783 template <class T> float operator()(const T vertex) const { return vertex[coord]; }
01784 };
01785
01787 struct GetCovar {
01788 int coord_a, coord_b;
01789 float mean_a, mean_b;
01790 template <class T> float operator()(const T vertex) const {
01791 return (vertex[coord_a] - mean_a) * (vertex[coord_b] - mean_b);
01792 }
01793 };
01794
01799 int
01800 computeDefaultCoG(std::vector<Triangle> &triangles, position &defaultCoG)
01801 {
01802
01803 double center_of_mass[3] = {0, 0, 0};
01804
01805 float total_area = 0;
01806 for (std::vector<Triangle>::const_iterator triangle = triangles.begin();
01807 triangle != triangles.end(); ++triangle) {
01808 total_area += triangle->area();
01809 }
01810
01811 if (total_area != 0) {
01812
01813 float means[3] = {0};
01814
01815 GetCoord get_coord;
01816 for(unsigned i = 0; i < 3; ++i) {
01817 get_coord.coord = i;
01818 for (std::vector<Triangle>::const_iterator triangle = triangles.begin();
01819 triangle != triangles.end(); ++triangle) {
01820 means[i] += Gaussian7Integrate(*triangle, get_coord);
01821 }
01822 }
01823 for (int i = 0; i < 3; ++i) center_of_mass[i] = means[i] / total_area;
01824
01825
01826 float covars[3][3] = {{0}};
01827 GetCovar get_covar;
01828 for(unsigned a = 0; a < 3; ++a) {
01829 for(unsigned b = a; b < 3; ++b) {
01830 get_covar.coord_a = a;
01831 get_covar.coord_b = b;
01832 get_covar.mean_a = center_of_mass[a];
01833 get_covar.mean_b = center_of_mass[b];
01834 for (std::vector<Triangle>::const_iterator triangle = triangles.begin();
01835 triangle != triangles.end(); ++triangle) {
01836 covars[b][a] += Gaussian7Integrate(*triangle, get_covar);
01837 }
01838 }
01839 }
01840 defaultCoG.set(center_of_mass);
01841 return SUCCESS;
01842 } else {
01843 defaultCoG.set(0,0,0);
01844 return FAILURE;
01845 }
01846 }
01852 void
01853 DynamicBody::computeDefaultMassProp(position &defaultCoG, double *defaultI)
01854 {
01855 std::vector<Triangle> triangles;
01856 getGeometryTriangles(&triangles);
01857 if (triangles.empty()) {
01858 DBGA("No triangles found when computing mass properties!");
01859 defaultCoG.set(0,0,0);
01860 return;
01861 }
01862 if (computeDefaultInertiaMatrix(triangles, defaultI) == FAILURE) {
01863 DBGA("Failed to compute inertia matrix based on geometry; using identity");
01864 }
01865 for(int i=0; i<3; i++) {
01866 DBGP(defaultI[3*i] << " " << defaultI[3*i+1] << " " << defaultI[3*i+2]);
01867 }
01868 computeDefaultCoG(triangles, defaultCoG);
01869 }
01870
01871 void
01872 DynamicBody::setDefaultViewingParameters()
01873 {
01874 Body::setDefaultViewingParameters();
01875 showAx = false;
01876 }
01877
01882 void
01883 DynamicBody::showAxes(bool on)
01884 {
01885 DBGP("Show axes: " << on);
01886 if (on) IVAxes->whichChild = 0;
01887 else IVAxes->whichChild = -1;
01888 showAx = on;
01889 }
01890
01895 void
01896 DynamicBody::showDynContactForces(bool on)
01897 {
01898 showDynCF = on;
01899 }
01900
01901
01905 void
01906 DynamicBody::resetExtWrenchAcc()
01907 {
01908 extWrenchAcc[0] = extWrenchAcc[1] = extWrenchAcc[2] = 0.0;
01909 extWrenchAcc[3] = extWrenchAcc[4] = extWrenchAcc[5] = 0.0;
01910 }
01911
01915 void
01916 DynamicBody::addExtWrench(double *extW){
01917 extWrenchAcc[0] += extW[0];
01918 extWrenchAcc[1] += extW[1];
01919 extWrenchAcc[2] += extW[2];
01920 extWrenchAcc[3] += extW[3];
01921 extWrenchAcc[4] += extW[4];
01922 extWrenchAcc[5] += extW[5];
01923 }
01924
01929 void
01930 DynamicBody::addForce(vec3 force)
01931 {
01932 extWrenchAcc[0] += force[0];
01933 extWrenchAcc[1] += force[1];
01934 extWrenchAcc[2] += force[2];
01935
01936 }
01937
01942 void
01943 DynamicBody::addTorque(vec3 torque)
01944 {
01945 extWrenchAcc[3] += torque[0];
01946 extWrenchAcc[4] += torque[1];
01947 extWrenchAcc[5] += torque[2];
01948 DBGP("Adding torque "<< torque);
01949 }
01950
01951
01952
01953
01954
01955 void
01956 DynamicBody::addRelTorque(vec3 torque)
01957 {
01958 vec3 worldTorque;
01959 DBGP("Adding rel torque "<< torque);
01960 worldTorque = Tran.rotation() * torque;
01961 extWrenchAcc[3] += worldTorque[0];
01962 extWrenchAcc[4] += worldTorque[1];
01963 extWrenchAcc[5] += worldTorque[2];
01964 DBGP(" world torque "<< worldTorque);
01965 }
01966
01967
01973 void
01974 DynamicBody::addForceAtPos(vec3 force,position pos)
01975 {
01976 vec3 worldTorque;
01977 vec3 worldPos;
01978 worldPos = (pos - CoG * Tran);
01979
01980 worldTorque = worldPos * force;
01981 extWrenchAcc[0] += force[0];
01982 extWrenchAcc[1] += force[1];
01983 extWrenchAcc[2] += force[2];
01984
01985 extWrenchAcc[3] += worldTorque[0];
01986 extWrenchAcc[4] += worldTorque[1];
01987 extWrenchAcc[5] += worldTorque[2];
01988 }
01989
01990
01996 void
01997 DynamicBody::addForceAtRelPos(vec3 force,position pos)
01998 {
01999 vec3 worldForce;
02000 vec3 worldTorque;
02001
02002 worldForce = Tran.rotation() * force;
02003 worldTorque = Tran.rotation() * ((pos - CoG) * force);
02004 extWrenchAcc[0] += worldForce[0];
02005 extWrenchAcc[1] += worldForce[1];
02006 extWrenchAcc[2] += worldForce[2];
02007
02008 extWrenchAcc[3] += worldTorque[0];
02009 extWrenchAcc[4] += worldTorque[1];
02010 extWrenchAcc[5] += worldTorque[2];
02011 DBGP("Adding rel force "<< force << " at " << pos);
02012 DBGP(" world torque "<< worldTorque << " worldForce " << worldForce);
02013 }
02014
02015
02016 bool
02017 DynamicBody::setPos(const double *new_q)
02018 {
02019 double norm;
02020
02021 if (new_q[0] < bbox_min.x() || new_q[0] > bbox_max.x()) return false;
02022 if (new_q[1] < bbox_min.y() || new_q[1] > bbox_max.y()) return false;
02023 if (new_q[2] < bbox_min.z() || new_q[2] > bbox_max.z()) return false;
02024
02025 memcpy(q,new_q,7*sizeof(double));
02026
02027
02028 norm = sqrt(q[3]*q[3]+q[4]*q[4]+q[5]*q[5]+q[6]*q[6]);
02029 q[3] /= norm;
02030 q[4] /= norm;
02031 q[5] /= norm;
02032 q[6] /= norm;
02033 Quaternion rot(q[3],q[4],q[5],q[6]);
02034 vec3 cogOffset = rot * (CoG-position::ORIGIN);
02035 transf tr = transf(rot,vec3(q[0],q[1],q[2])-cogOffset);
02036
02037 Tran = tr;
02038 Tran.toSoTransform(IVTran);
02039 myWorld->getCollisionInterface()->setBodyTransform(this, Tran);
02040
02041 return true;
02042 }
02043
02048 void
02049 DynamicBody::markState()
02050 {
02051 memcpy(markedQ,q,7*sizeof(double));
02052 memcpy(markedV,v,6*sizeof(double));
02053 }
02054
02059 void
02060 DynamicBody::returnToMarkedState()
02061 {
02062 memcpy(v,markedV,6*sizeof(double));
02063 setPos(markedQ);
02064 }
02065
02069 void
02070 DynamicBody::pushState()
02071 {
02072 double *tmp = new double[7];
02073 memcpy(tmp,q,7*sizeof(double));
02074 qStack.push_back(tmp);
02075
02076 tmp = new double[6];
02077 memcpy(tmp,v,6*sizeof(double));
02078 vStack.push_back(tmp);
02079 }
02080
02084 bool
02085 DynamicBody::popState()
02086 {
02087 if (qStack.empty()) {
02088 DBGA("Pop state failed: stack empty");
02089 return false;
02090 }
02091 memcpy(v,vStack.back(),6*sizeof(double));
02092 setPos(qStack.back());
02093
02094
02095 if (++qStack.begin() != qStack.end()) {
02096 delete [] vStack.back();
02097 delete [] qStack.back();
02098 vStack.pop_back();
02099 qStack.pop_back();
02100 return true;
02101 } else {
02102 return false;
02103 }
02104 }
02105
02106 void
02107 DynamicBody::clearState()
02108 {
02109
02110
02111 if (qStack.empty()) return;
02112 while (++qStack.begin() != qStack.end()) {
02113 delete [] vStack.back();
02114 delete [] qStack.back();
02115 vStack.pop_back();
02116 qStack.pop_back();
02117 }
02118 }
02122 int
02123 DynamicBody::setTran(transf const& tr)
02124 {
02125 if (tr == Tran) return SUCCESS;
02126 if (Body::setTran(tr) == FAILURE) return FAILURE;
02127 Quaternion quat = Tran.rotation();
02128 vec3 cogOffset = quat * (CoG-position::ORIGIN);
02129 q[0] = Tran.translation().x()+cogOffset.x();
02130 q[1] = Tran.translation().y()+cogOffset.y();
02131 q[2] = Tran.translation().z()+cogOffset.z();
02132 q[3] = quat.w;
02133 q[4] = quat.x;
02134 q[5] = quat.y;
02135 q[6] = quat.z;
02136 if (fixed) {
02137 if (!dynJoint->getPrevLink()) {
02138
02139
02140
02141 fix();
02142 }
02143 }
02144 return SUCCESS;
02145 }
02146
02152 void
02153 DynamicBody::fix()
02154 {
02155 fixed = true;
02156 setDynJoint(new FixedDynJoint(NULL,this,Tran));
02157 }
02158
02163 void
02164 DynamicBody::unfix()
02165 {
02166 fixed = false;
02167 setDynJoint(NULL);
02168 }
02169
02173 void
02174 DynamicBody::setDynJoint(DynJoint *dj)
02175 {
02176 if (dynJoint) delete dynJoint;
02177 dynJoint = dj;
02178 }
02179
02181
02183
02184
02185
02186
02187
02188
02189 Link::Link(Robot *r,int c, int l,World *w,const char *name) : DynamicBody(w,name)
02190 {
02191 owner = r; chainNum = c; linkNum = l; showVC = false; showFC = false;
02192 }
02193
02194
02195
02196
02197
02198 Link::~Link()
02199 {
02200 }
02201
02202
02206 void
02207 Link::setContactsChanged()
02208 {
02209 WorldElement::setContactsChanged();
02210 owner->setContactsChanged();
02211 }
02217 bool
02218 Link::externalContactsPreventMotion(const transf& motion)
02219 {
02220 std::list<Contact *>::iterator cp;
02221 std::list<Contact *> contactList;
02222
02223 contactList = getContacts();
02224 for (cp=contactList.begin();cp!=contactList.end();cp++) {
02225 if ( (*cp)->getBody2()->getOwner() == getOwner() )
02226 continue;
02227 if ((*cp)->preventsMotion(motion)) {
02228 return true;
02229 }
02230 }
02231 return false;
02232 }
02233
02235 position
02236 Link::getDistalJointLocation()
02237 {
02238 return position(0,0,0);
02239 }
02240
02243 position
02244 Link::getProximalJointLocation()
02245 {
02246 int jointNum = owner->getChain(chainNum)->getLastJoint(linkNum);
02247 Joint *j = owner->getChain(chainNum)->getJoint(jointNum);
02248 vec3 p = j->getTran().inverse().translation();
02249 return position( p.x(), p.y(), p.z() );
02250 }
02251
02252 vec3
02253 Link::getProximalJointAxis()
02254 {
02255 int jointNum = owner->getChain(chainNum)->getLastJoint(linkNum);
02256 Joint *j = owner->getChain(chainNum)->getJoint(jointNum);
02257 vec3 r = vec3(0,0,1) * j->getTran().inverse();
02258 return r;
02259 }
02260
02262
02264
02265
02266
02267
02268 GraspableBody::GraspableBody(World *w,const char *name) : DynamicBody(w,name)
02269 {
02270 #ifdef CGDB_ENABLED
02271 mGraspitDBModel = NULL;
02272 #endif
02273 }
02274
02275
02276
02277
02278 GraspableBody::~GraspableBody()
02279 {
02280
02281 }
02282
02284 void
02285 GraspableBody::setDefaultViewingParameters()
02286 {
02287 showFC = true;
02288 showVC = true;
02289 showAxes(true);
02290 setTransparency(0.4f);
02291 }
02292
02294 void
02295 GraspableBody::cloneFrom(const GraspableBody *original)
02296 {
02297 DynamicBody::cloneFrom(original);
02298 setDefaultViewingParameters();
02299 }
02300
02304 QTextStream&
02305 operator<<(QTextStream &os, const GraspableBody &gb)
02306 {
02307 os << gb.myFilename << endl;
02308 return os;
02309 }
