1 /* Copyright (C) 2015 Google
2
3 This software is provided 'as-is', without any express or implied warranty.
4 In no event will the authors be held liable for any damages arising from the use of this software.
5 Permission is granted to anyone to use this software for any purpose,
6 including commercial applications, and to alter it and redistribute it freely,
7 subject to the following restrictions:
8
9 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
10 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
11 3. This notice may not be removed or altered from any source distribution.
12 */
13
14 #include "PhysXUrdfImporter.h"
15 #include "../../CommonInterfaces/CommonRenderInterface.h"
16 #include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
17 #include "../../Importers/ImportURDFDemo/URDFImporterInterface.h"
18
19 #include "../../Importers/ImportObjDemo/LoadMeshFromObj.h"
20 #include "../../Importers/ImportSTLDemo/LoadMeshFromSTL.h"
21 #include "../../Importers/ImportColladaDemo/LoadMeshFromCollada.h"
22 //#include "BulletCollision/CollisionShapes/btShapeHull.h" //to create a tesselation of a generic btConvexShape
23 #include "../../CommonInterfaces/CommonGUIHelperInterface.h"
24 #include "../../CommonInterfaces/CommonFileIOInterface.h"
25 #include "Bullet3Common/b3FileUtils.h"
26 #include <string>
27 #include "../../Utils/b3ResourcePath.h"
28 #include "../../Utils/b3BulletDefaultFileIO.h"
29
30 #include "../OpenGLWindow/ShapeData.h"
31
32
33 #include "../../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
34
35 static btScalar gUrdfDefaultCollisionMargin = 0.001;
36
37 #include <iostream>
38 #include <fstream>
39 #include <list>
40 #include "../../Importers/ImportURDFDemo/URDFJointTypes.h"
41 #include "../../Importers/ImportURDFDemo/UrdfParser.h"
42
43
ATTRIBUTE_ALIGNED16(struct)44 ATTRIBUTE_ALIGNED16(struct)
45 PhysXURDFInternalData
46 {
47 BT_DECLARE_ALIGNED_ALLOCATOR();
48 b3BulletDefaultFileIO m_defaultFileIO;
49 UrdfParser m_urdfParser;
50 struct GUIHelperInterface* m_guiHelper;
51 struct CommonFileIOInterface* m_fileIO;
52 std::string m_sourceFile;
53 char m_pathPrefix[1024];
54 int m_bodyId;
55 btHashMap<btHashInt, UrdfMaterialColor> m_linkColors;
56 btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
57 btAlignedObjectArray<int> m_allocatedTextures;
58 //mutable btAlignedObjectArray<btTriangleMesh*> m_allocatedMeshInterfaces;
59 btHashMap<btHashPtr, UrdfCollision> m_bulletCollisionShape2UrdfCollision;
60
61 UrdfRenderingInterface* m_customVisualShapesConverter;
62 bool m_enableTinyRenderer;
63 int m_flags;
64
65 void setSourceFile(const std::string& relativeFileName, const std::string& prefix)
66 {
67 m_sourceFile = relativeFileName;
68 m_urdfParser.setSourceFile(relativeFileName);
69 strncpy(m_pathPrefix, prefix.c_str(), sizeof(m_pathPrefix));
70 m_pathPrefix[sizeof(m_pathPrefix) - 1] = 0; // required, strncpy doesn't write zero on overflow
71 }
72
73 PhysXURDFInternalData(CommonFileIOInterface* fileIO)
74 :m_urdfParser(fileIO? fileIO : &m_defaultFileIO),
75 m_fileIO(fileIO? fileIO : &m_defaultFileIO)
76 {
77 m_enableTinyRenderer = true;
78 m_pathPrefix[0] = 0;
79 m_flags = 0;
80 }
81
82 void setGlobalScaling(btScalar scaling)
83 {
84 m_urdfParser.setGlobalScaling(scaling);
85 }
86
87
88 };
89
printTree()90 void PhysXURDFImporter::printTree()
91 {
92 // btAssert(0);
93 }
94
95
96
97
PhysXURDFImporter(struct CommonFileIOInterface * fileIO,double globalScaling,int flags)98 PhysXURDFImporter::PhysXURDFImporter(struct CommonFileIOInterface* fileIO,double globalScaling, int flags)
99 {
100 m_data = new PhysXURDFInternalData(fileIO);
101 m_data->setGlobalScaling(globalScaling);
102 m_data->m_flags = flags;
103 }
104
105 struct PhysXErrorLogger : public ErrorLogger
106 {
107 int m_numErrors;
108 int m_numWarnings;
109
PhysXErrorLoggerPhysXErrorLogger110 PhysXErrorLogger()
111 : m_numErrors(0),
112 m_numWarnings(0)
113 {
114 }
reportErrorPhysXErrorLogger115 virtual void reportError(const char* error)
116 {
117 m_numErrors++;
118 b3Error(error);
119 }
reportWarningPhysXErrorLogger120 virtual void reportWarning(const char* warning)
121 {
122 m_numWarnings++;
123 b3Warning(warning);
124 }
125
printMessagePhysXErrorLogger126 virtual void printMessage(const char* msg)
127 {
128 b3Printf(msg);
129 }
130 };
131
loadURDF(const char * fileName,bool forceFixedBase)132 bool PhysXURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
133 {
134 if (strlen(fileName) == 0)
135 return false;
136
137 //int argc=0;
138 char relativeFileName[1024];
139
140 b3FileUtils fu;
141
142 //bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
143 bool fileFound = m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024);
144
145 std::string xml_string;
146
147 if (!fileFound)
148 {
149 b3Warning("URDF file '%s' not found\n", fileName);
150 return false;
151 }
152 else
153 {
154 char path[1024];
155 fu.extractPath(relativeFileName, path, sizeof(path));
156 m_data->setSourceFile(relativeFileName, path);
157
158 //read file
159 int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r");
160
161
162 char destBuffer[8192];
163 char* line = 0;
164 do
165 {
166 line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192);
167 if (line)
168 {
169 xml_string += (std::string(destBuffer) + "\n");
170 }
171 }
172 while (line);
173 m_data->m_fileIO->fileClose(fileId);
174 #if 0
175 std::fstream xml_file(relativeFileName, std::fstream::in);
176 while (xml_file.good())
177 {
178 std::string line;
179 std::getline(xml_file, line);
180 xml_string += (line + "\n");
181 }
182 xml_file.close();
183 #endif
184
185 }
186
187 PhysXErrorLogger loggie;
188 m_data->m_urdfParser.setParseSDF(false);
189 bool result = false;
190
191 if (xml_string.length())
192 {
193 result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase, (m_data->m_flags & CUF_PARSE_SENSORS));
194 }
195
196 return result;
197 }
198
getNumModels() const199 int PhysXURDFImporter::getNumModels() const
200 {
201 return m_data->m_urdfParser.getNumModels();
202 }
203
activateModel(int modelIndex)204 void PhysXURDFImporter::activateModel(int modelIndex)
205 {
206 m_data->m_urdfParser.activateModel(modelIndex);
207 }
208
loadSDF(const char * fileName,bool forceFixedBase)209 bool PhysXURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
210 {
211 //int argc=0;
212 char relativeFileName[1024];
213
214 b3FileUtils fu;
215
216 //bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
217 bool fileFound = (m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024));
218
219 std::string xml_string;
220
221 if (!fileFound)
222 {
223 b3Warning("SDF file '%s' not found\n", fileName);
224 return false;
225 }
226 else
227 {
228
229 char path[1024];
230 fu.extractPath(relativeFileName, path, sizeof(path));
231 m_data->setSourceFile(relativeFileName, path);
232
233 //read file
234 int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r");
235
236 char destBuffer[8192];
237 char* line = 0;
238 do
239 {
240 line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192);
241 if (line)
242 {
243 xml_string += (std::string(destBuffer) + "\n");
244 }
245 }
246 while (line);
247 m_data->m_fileIO->fileClose(fileId);
248 }
249
250 PhysXErrorLogger loggie;
251 //todo: quick test to see if we can re-use the URDF parser for SDF or not
252 m_data->m_urdfParser.setParseSDF(true);
253 bool result = false;
254 if (xml_string.length())
255 {
256 result = m_data->m_urdfParser.loadSDF(xml_string.c_str(), &loggie);
257 }
258
259 return result;
260 }
261
getPathPrefix()262 const char* PhysXURDFImporter::getPathPrefix()
263 {
264 return m_data->m_pathPrefix;
265 }
266
setBodyUniqueId(int bodyId)267 void PhysXURDFImporter::setBodyUniqueId(int bodyId)
268 {
269 m_data->m_bodyId = bodyId;
270 }
271
getBodyUniqueId() const272 int PhysXURDFImporter::getBodyUniqueId() const
273 {
274 return m_data->m_bodyId;
275 }
276
~PhysXURDFImporter()277 PhysXURDFImporter::~PhysXURDFImporter()
278 {
279 delete m_data;
280 }
281
getRootLinkIndex() const282 int PhysXURDFImporter::getRootLinkIndex() const
283 {
284 if (m_data->m_urdfParser.getModel().m_rootLinks.size() == 1)
285 {
286 return m_data->m_urdfParser.getModel().m_rootLinks[0]->m_linkIndex;
287 }
288 return -1;
289 };
290
getLinkChildIndices(int linkIndex,btAlignedObjectArray<int> & childLinkIndices) const291 void PhysXURDFImporter::getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const
292 {
293 childLinkIndices.resize(0);
294 UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
295 if (linkPtr)
296 {
297 const UrdfLink* link = *linkPtr;
298 //int numChildren = m_data->m_urdfParser->getModel().m_links.getAtIndex(linkIndex)->
299
300 for (int i = 0; i < link->m_childLinks.size(); i++)
301 {
302 int childIndex = link->m_childLinks[i]->m_linkIndex;
303 childLinkIndices.push_back(childIndex);
304 }
305 }
306 }
307
getLinkName(int linkIndex) const308 std::string PhysXURDFImporter::getLinkName(int linkIndex) const
309 {
310 UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
311 btAssert(linkPtr);
312 if (linkPtr)
313 {
314 UrdfLink* link = *linkPtr;
315 return link->m_name;
316 }
317 return "";
318 }
319
getBodyName() const320 std::string PhysXURDFImporter::getBodyName() const
321 {
322 return m_data->m_urdfParser.getModel().m_name;
323 }
324
getJointName(int linkIndex) const325 std::string PhysXURDFImporter::getJointName(int linkIndex) const
326 {
327 UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
328 btAssert(linkPtr);
329 if (linkPtr)
330 {
331 UrdfLink* link = *linkPtr;
332 if (link->m_parentJoint)
333 {
334 return link->m_parentJoint->m_name;
335 }
336 }
337 return "";
338 }
339
getMassAndInertia2(int urdfLinkIndex,btScalar & mass,btVector3 & localInertiaDiagonal,btTransform & inertialFrame,int flags) const340 void PhysXURDFImporter::getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const
341 {
342 if (flags & CUF_USE_URDF_INERTIA)
343 {
344 getMassAndInertia(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrame);
345 }
346 else
347 {
348 //the link->m_inertia is NOT necessarily aligned with the inertial frame
349 //so an additional transform might need to be computed
350 UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex);
351
352 btAssert(linkPtr);
353 if (linkPtr)
354 {
355 UrdfLink* link = *linkPtr;
356 btScalar linkMass;
357 if (link->m_parentJoint == 0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
358 {
359 linkMass = 0.f;
360 }
361 else
362 {
363 linkMass = link->m_inertia.m_mass;
364 }
365 mass = linkMass;
366 localInertiaDiagonal.setValue(0, 0, 0);
367 inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin());
368 inertialFrame.setBasis(link->m_inertia.m_linkLocalFrame.getBasis());
369 }
370 else
371 {
372 mass = 1.f;
373 localInertiaDiagonal.setValue(1, 1, 1);
374 inertialFrame.setIdentity();
375 }
376 }
377 }
378
getMassAndInertia(int linkIndex,btScalar & mass,btVector3 & localInertiaDiagonal,btTransform & inertialFrame) const379 void PhysXURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
380 {
381 //the link->m_inertia is NOT necessarily aligned with the inertial frame
382 //so an additional transform might need to be computed
383 UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
384
385 btAssert(linkPtr);
386 if (linkPtr)
387 {
388 UrdfLink* link = *linkPtr;
389 btMatrix3x3 linkInertiaBasis;
390 btScalar linkMass, principalInertiaX, principalInertiaY, principalInertiaZ;
391 if (link->m_parentJoint == 0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
392 {
393 linkMass = 0.f;
394 principalInertiaX = 0.f;
395 principalInertiaY = 0.f;
396 principalInertiaZ = 0.f;
397 linkInertiaBasis.setIdentity();
398 }
399 else
400 {
401 linkMass = link->m_inertia.m_mass;
402 if (link->m_inertia.m_ixy == 0.0 &&
403 link->m_inertia.m_ixz == 0.0 &&
404 link->m_inertia.m_iyz == 0.0)
405 {
406 principalInertiaX = link->m_inertia.m_ixx;
407 principalInertiaY = link->m_inertia.m_iyy;
408 principalInertiaZ = link->m_inertia.m_izz;
409 linkInertiaBasis.setIdentity();
410 }
411 else
412 {
413 principalInertiaX = link->m_inertia.m_ixx;
414 btMatrix3x3 inertiaTensor(link->m_inertia.m_ixx, link->m_inertia.m_ixy, link->m_inertia.m_ixz,
415 link->m_inertia.m_ixy, link->m_inertia.m_iyy, link->m_inertia.m_iyz,
416 link->m_inertia.m_ixz, link->m_inertia.m_iyz, link->m_inertia.m_izz);
417 btScalar threshold = 1.0e-6;
418 int numIterations = 30;
419 inertiaTensor.diagonalize(linkInertiaBasis, threshold, numIterations);
420 principalInertiaX = inertiaTensor[0][0];
421 principalInertiaY = inertiaTensor[1][1];
422 principalInertiaZ = inertiaTensor[2][2];
423 }
424 }
425 mass = linkMass;
426 if (principalInertiaX < 0 ||
427 principalInertiaX > (principalInertiaY + principalInertiaZ) ||
428 principalInertiaY < 0 ||
429 principalInertiaY > (principalInertiaX + principalInertiaZ) ||
430 principalInertiaZ < 0 ||
431 principalInertiaZ > (principalInertiaX + principalInertiaY))
432 {
433 b3Warning("Bad inertia tensor properties, setting inertia to zero for link: %s\n", link->m_name.c_str());
434 principalInertiaX = 0.f;
435 principalInertiaY = 0.f;
436 principalInertiaZ = 0.f;
437 linkInertiaBasis.setIdentity();
438 }
439 localInertiaDiagonal.setValue(principalInertiaX, principalInertiaY, principalInertiaZ);
440 inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin());
441 inertialFrame.setBasis(link->m_inertia.m_linkLocalFrame.getBasis() * linkInertiaBasis);
442 }
443 else
444 {
445 mass = 1.f;
446 localInertiaDiagonal.setValue(1, 1, 1);
447 inertialFrame.setIdentity();
448 }
449 }
450
getJointInfo2(int urdfLinkIndex,btTransform & parent2joint,btTransform & linkTransformInWorld,btVector3 & jointAxisInJointSpace,int & jointType,btScalar & jointLowerLimit,btScalar & jointUpperLimit,btScalar & jointDamping,btScalar & jointFriction,btScalar & jointMaxForce,btScalar & jointMaxVelocity) const451 bool PhysXURDFImporter::getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const
452 {
453 jointLowerLimit = 0.f;
454 jointUpperLimit = 0.f;
455 jointDamping = 0.f;
456 jointFriction = 0.f;
457 jointMaxForce = 0.f;
458 jointMaxVelocity = 0.f;
459
460 UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex);
461 btAssert(linkPtr);
462 if (linkPtr)
463 {
464 UrdfLink* link = *linkPtr;
465 linkTransformInWorld = link->m_linkTransformInWorld;
466
467 if (link->m_parentJoint)
468 {
469 UrdfJoint* pj = link->m_parentJoint;
470 parent2joint = pj->m_parentLinkToJointTransform;
471 jointType = pj->m_type;
472 jointAxisInJointSpace = pj->m_localJointAxis;
473 jointLowerLimit = pj->m_lowerLimit;
474 jointUpperLimit = pj->m_upperLimit;
475 jointDamping = pj->m_jointDamping;
476 jointFriction = pj->m_jointFriction;
477 jointMaxForce = pj->m_effortLimit;
478 jointMaxVelocity = pj->m_velocityLimit;
479 return true;
480 }
481 else
482 {
483 parent2joint.setIdentity();
484 return false;
485 }
486 }
487
488 return false;
489 };
490
getJointInfo(int urdfLinkIndex,btTransform & parent2joint,btTransform & linkTransformInWorld,btVector3 & jointAxisInJointSpace,int & jointType,btScalar & jointLowerLimit,btScalar & jointUpperLimit,btScalar & jointDamping,btScalar & jointFriction) const491 bool PhysXURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
492 {
493 btScalar jointMaxForce;
494 btScalar jointMaxVelocity;
495 return getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity);
496 }
497
setRootTransformInWorld(const btTransform & rootTransformInWorld)498 void PhysXURDFImporter::setRootTransformInWorld(const btTransform& rootTransformInWorld)
499 {
500 m_data->m_urdfParser.getModel().m_rootTransformInWorld = rootTransformInWorld;
501 }
502
getRootTransformInWorld(btTransform & rootTransformInWorld) const503 bool PhysXURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const
504 {
505 rootTransformInWorld = m_data->m_urdfParser.getModel().m_rootTransformInWorld;
506 return true;
507 }
508
509
getUrdfLink(int urdfLinkIndex) const510 const struct UrdfLink* PhysXURDFImporter::getUrdfLink(int urdfLinkIndex) const
511 {
512 UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex);
513 btAssert(linkPtr);
514 if (linkPtr)
515 {
516 const UrdfLink* link = *linkPtr;
517 return link;
518 }
519 return 0;
520 }
521
getUrdfModel() const522 const struct UrdfModel* PhysXURDFImporter::getUrdfModel() const
523 {
524 return &m_data->m_urdfParser.getModel();
525 }
526
527
getUrdfFromCollisionShape(const btCollisionShape * collisionShape,UrdfCollision & collision) const528 int PhysXURDFImporter::getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const
529 {
530 UrdfCollision* col = m_data->m_bulletCollisionShape2UrdfCollision.find(collisionShape);
531 if (col)
532 {
533 collision = *col;
534 return 1;
535 }
536 return 0;
537 }
538
539 #if 0
540 btCollisionShape* PhysXURDFImporter::convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix) const
541 {
542
543 B3_PROFILE("convertURDFToCollisionShape");
544
545 btCollisionShape* shape = 0;
546
547 switch (collision->m_geometry.m_type)
548 {
549 case URDF_GEOM_PLANE:
550 {
551 btVector3 planeNormal = collision->m_geometry.m_planeNormal;
552 btScalar planeConstant = 0; //not available?
553 btStaticPlaneShape* plane = new btStaticPlaneShape(planeNormal, planeConstant);
554 shape = plane;
555 shape->setMargin(gUrdfDefaultCollisionMargin);
556 break;
557 }
558 case URDF_GEOM_CAPSULE:
559 {
560 btScalar radius = collision->m_geometry.m_capsuleRadius;
561 btScalar height = collision->m_geometry.m_capsuleHeight;
562 btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius, height);
563 shape = capsuleShape;
564 shape->setMargin(gUrdfDefaultCollisionMargin);
565 break;
566 }
567
568 case URDF_GEOM_CYLINDER:
569 {
570 btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
571 btScalar cylHalfLength = 0.5 * collision->m_geometry.m_capsuleHeight;
572 if (m_data->m_flags & CUF_USE_IMPLICIT_CYLINDER)
573 {
574 btVector3 halfExtents(cylRadius, cylRadius, cylHalfLength);
575 btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
576 shape = cylZShape;
577 }
578 else
579 {
580 btAlignedObjectArray<btVector3> vertices;
581 //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
582 int numSteps = 32;
583 for (int i = 0; i < numSteps; i++)
584 {
585 btVector3 vert(cylRadius * btSin(SIMD_2_PI * (float(i) / numSteps)), cylRadius * btCos(SIMD_2_PI * (float(i) / numSteps)), cylHalfLength);
586 vertices.push_back(vert);
587 vert[2] = -cylHalfLength;
588 vertices.push_back(vert);
589 }
590 btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
591 cylZShape->setMargin(gUrdfDefaultCollisionMargin);
592 cylZShape->recalcLocalAabb();
593 if (m_data->m_flags & CUF_INITIALIZE_SAT_FEATURES)
594 {
595 cylZShape->initializePolyhedralFeatures();
596 }
597 cylZShape->optimizeConvexHull();
598 shape = cylZShape;
599 }
600
601 break;
602 }
603 case URDF_GEOM_BOX:
604 {
605 btVector3 extents = collision->m_geometry.m_boxSize;
606 btBoxShape* boxShape = new btBoxShape(extents * 0.5f);
607 //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
608 if (m_data->m_flags & CUF_INITIALIZE_SAT_FEATURES)
609 {
610 boxShape->initializePolyhedralFeatures();
611 }
612 shape = boxShape;
613 shape->setMargin(gUrdfDefaultCollisionMargin);
614 break;
615 }
616 case URDF_GEOM_SPHERE:
617 {
618 btScalar radius = collision->m_geometry.m_sphereRadius;
619 btSphereShape* sphereShape = new btSphereShape(radius);
620 shape = sphereShape;
621 shape->setMargin(gUrdfDefaultCollisionMargin);
622 break;
623 }
624 case URDF_GEOM_CDF:
625 {
626 char relativeFileName[1024];
627 char pathPrefix[1024];
628 pathPrefix[0] = 0;
629 if (m_data->m_fileIO->findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024))
630 {
631 b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
632
633 btAlignedObjectArray<char> sdfData;
634 {
635 std::streampos fsize = 0;
636 std::ifstream file(relativeFileName, std::ios::binary);
637 if (file.good())
638 {
639 fsize = file.tellg();
640 file.seekg(0, std::ios::end);
641 fsize = file.tellg() - fsize;
642 file.seekg(0, std::ios::beg);
643 sdfData.resize(fsize);
644 int bytesRead = file.rdbuf()->sgetn(&sdfData[0], fsize);
645 btAssert(bytesRead == fsize);
646 file.close();
647 }
648 }
649
650 if (sdfData.size())
651 {
652 btSdfCollisionShape* sdfShape = new btSdfCollisionShape();
653 bool valid = sdfShape->initializeSDF(&sdfData[0], sdfData.size());
654 btAssert(valid);
655
656 if (valid)
657 {
658 shape = sdfShape;
659 }
660 else
661 {
662 delete sdfShape;
663 }
664 }
665 }
666 break;
667 }
668 case URDF_GEOM_MESH:
669 {
670 GLInstanceGraphicsShape* glmesh = 0;
671 switch (collision->m_geometry.m_meshFileType)
672 {
673 case UrdfGeometry::FILE_OBJ:
674 if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
675 {
676 char relativeFileName[1024];
677 char pathPrefix[1024];
678 pathPrefix[0] = 0;
679 if (m_data->m_fileIO->findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024))
680 {
681 b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
682 }
683 glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), pathPrefix,m_data->m_fileIO);
684 }
685 else
686 {
687 std::vector<tinyobj::shape_t> shapes;
688 std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO);
689 //create a convex hull for each shape, and store it in a btCompoundShape
690
691 btAssert(0);
692
693 return shape;
694 }
695 break;
696
697 case UrdfGeometry::FILE_STL:
698 glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str(), m_data->m_fileIO);
699 break;
700
701 case UrdfGeometry::FILE_COLLADA:
702 {
703 btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
704 btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
705 btTransform upAxisTrans;
706 upAxisTrans.setIdentity();
707 float unitMeterScaling = 1;
708 LoadMeshFromCollada(collision->m_geometry.m_meshFileName.c_str(), visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, 2, m_data->m_fileIO);
709
710 glmesh = new GLInstanceGraphicsShape;
711 glmesh->m_indices = new b3AlignedObjectArray<int>();
712 glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
713
714 for (int i = 0; i < visualShapeInstances.size(); i++)
715 {
716 ColladaGraphicsInstance* instance = &visualShapeInstances[i];
717 GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
718
719 b3AlignedObjectArray<GLInstanceVertex> verts;
720 verts.resize(gfxShape->m_vertices->size());
721
722 int baseIndex = glmesh->m_vertices->size();
723
724 for (int i = 0; i < gfxShape->m_vertices->size(); i++)
725 {
726 verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
727 verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
728 verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
729 verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
730 verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
731 verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
732 verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
733 verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
734 verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
735 }
736
737 int curNumIndices = glmesh->m_indices->size();
738 int additionalIndices = gfxShape->m_indices->size();
739 glmesh->m_indices->resize(curNumIndices + additionalIndices);
740 for (int k = 0; k < additionalIndices; k++)
741 {
742 glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
743 }
744
745 //compensate upAxisTrans and unitMeterScaling here
746 btMatrix4x4 upAxisMat;
747 upAxisMat.setIdentity();
748 //upAxisMat.setPureRotation(upAxisTrans.getRotation());
749 btMatrix4x4 unitMeterScalingMat;
750 unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
751 btMatrix4x4 worldMat = unitMeterScalingMat * instance->m_worldTransform * upAxisMat;
752 //btMatrix4x4 worldMat = instance->m_worldTransform;
753 int curNumVertices = glmesh->m_vertices->size();
754 int additionalVertices = verts.size();
755 glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
756
757 for (int v = 0; v < verts.size(); v++)
758 {
759 btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
760 pos = worldMat * pos;
761 verts[v].xyzw[0] = float(pos[0]);
762 verts[v].xyzw[1] = float(pos[1]);
763 verts[v].xyzw[2] = float(pos[2]);
764 glmesh->m_vertices->push_back(verts[v]);
765 }
766 }
767 glmesh->m_numIndices = glmesh->m_indices->size();
768 glmesh->m_numvertices = glmesh->m_vertices->size();
769 //glmesh = LoadMeshFromCollada(success.c_str());
770 break;
771 }
772 }
773
774 if (!glmesh || glmesh->m_numvertices <= 0)
775 {
776 b3Warning("%s: cannot extract mesh from '%s'\n", urdfPathPrefix, collision->m_geometry.m_meshFileName.c_str());
777 delete glmesh;
778 break;
779 }
780
781 btAlignedObjectArray<btVector3> convertedVerts;
782 convertedVerts.reserve(glmesh->m_numvertices);
783 for (int i = 0; i < glmesh->m_numvertices; i++)
784 {
785 convertedVerts.push_back(btVector3(
786 glmesh->m_vertices->at(i).xyzw[0] * collision->m_geometry.m_meshScale[0],
787 glmesh->m_vertices->at(i).xyzw[1] * collision->m_geometry.m_meshScale[1],
788 glmesh->m_vertices->at(i).xyzw[2] * collision->m_geometry.m_meshScale[2]));
789 }
790
791 if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
792 {
793 BT_PROFILE("convert trimesh");
794 btTriangleMesh* meshInterface = new btTriangleMesh();
795 m_data->m_allocatedMeshInterfaces.push_back(meshInterface);
796 {
797 BT_PROFILE("convert vertices");
798
799 for (int i = 0; i < glmesh->m_numIndices / 3; i++)
800 {
801 const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i * 3)];
802 const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i * 3 + 1)];
803 const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i * 3 + 2)];
804 meshInterface->addTriangle(v0, v1, v2);
805 }
806 }
807 {
808 BT_PROFILE("create btBvhTriangleMeshShape");
809 btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true);
810 //trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
811 shape = trimesh;
812 }
813 }
814 else
815 {
816 BT_PROFILE("convert btConvexHullShape");
817 btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
818 convexHull->optimizeConvexHull();
819 if (m_data->m_flags & CUF_INITIALIZE_SAT_FEATURES)
820 {
821 convexHull->initializePolyhedralFeatures();
822 }
823 convexHull->setMargin(gUrdfDefaultCollisionMargin);
824 convexHull->recalcLocalAabb();
825 //convexHull->setLocalScaling(collision->m_geometry.m_meshScale);
826 shape = convexHull;
827 }
828
829 delete glmesh;
830 break;
831 } // mesh case
832
833 default:
834 b3Warning("Error: unknown collision geometry type %i\n", collision->m_geometry.m_type);
835 }
836 if (shape && collision->m_geometry.m_type == URDF_GEOM_MESH)
837 {
838 m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision);
839 }
840 return shape;
841 }
842
843
844 void PhysXURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<BulletURDFTexture>& texturesOut, struct b3ImportMeshData& meshData) const
845 {
846 BT_PROFILE("convertURDFToVisualShapeInternal");
847
848 GLInstanceGraphicsShape* glmesh = 0;
849
850 btConvexShape* convexColShape = 0;
851
852 switch (visual->m_geometry.m_type)
853 {
854 case URDF_GEOM_CAPSULE:
855 {
856 btScalar radius = visual->m_geometry.m_capsuleRadius;
857 btScalar height = visual->m_geometry.m_capsuleHeight;
858 btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius, height);
859 convexColShape = capsuleShape;
860 convexColShape->setMargin(gUrdfDefaultCollisionMargin);
861 break;
862 }
863 case URDF_GEOM_CYLINDER:
864 {
865 btAlignedObjectArray<btVector3> vertices;
866
867 //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
868 int numSteps = 32;
869 for (int i = 0; i < numSteps; i++)
870 {
871 btScalar cylRadius = visual->m_geometry.m_capsuleRadius;
872 btScalar cylLength = visual->m_geometry.m_capsuleHeight;
873
874 btVector3 vert(cylRadius * btSin(SIMD_2_PI * (float(i) / numSteps)), cylRadius * btCos(SIMD_2_PI * (float(i) / numSteps)), cylLength / 2.);
875 vertices.push_back(vert);
876 vert[2] = -cylLength / 2.;
877 vertices.push_back(vert);
878 }
879
880 btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
881 cylZShape->setMargin(gUrdfDefaultCollisionMargin);
882 cylZShape->recalcLocalAabb();
883 convexColShape = cylZShape;
884 break;
885 }
886
887 case URDF_GEOM_BOX:
888 {
889 btVector3 extents = visual->m_geometry.m_boxSize;
890 int strideInBytes = 9 * sizeof(float);
891 int numVertices = sizeof(cube_vertices_textured) / strideInBytes;
892 int numIndices = sizeof(cube_indices) / sizeof(int);
893 glmesh = new GLInstanceGraphicsShape;
894 glmesh->m_indices = new b3AlignedObjectArray<int>();
895 glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
896 glmesh->m_indices->resize(numIndices);
897 for (int k = 0; k < numIndices; k++)
898 {
899 glmesh->m_indices->at(k) = cube_indices[k];
900 }
901 glmesh->m_vertices->resize(numVertices);
902
903 btScalar halfExtentsX = extents[0] * 0.5;
904 btScalar halfExtentsY = extents[1] * 0.5;
905 btScalar halfExtentsZ = extents[2] * 0.5;
906 GLInstanceVertex* verts = &glmesh->m_vertices->at(0);
907 btScalar textureScaling = 1;
908
909 for (int i = 0; i < numVertices; i++)
910 {
911 verts[i].xyzw[0] = halfExtentsX * cube_vertices_textured[i * 9];
912 verts[i].xyzw[1] = halfExtentsY * cube_vertices_textured[i * 9 + 1];
913 verts[i].xyzw[2] = halfExtentsZ * cube_vertices_textured[i * 9 + 2];
914 verts[i].xyzw[3] = cube_vertices_textured[i * 9 + 3];
915 verts[i].normal[0] = cube_vertices_textured[i * 9 + 4];
916 verts[i].normal[1] = cube_vertices_textured[i * 9 + 5];
917 verts[i].normal[2] = cube_vertices_textured[i * 9 + 6];
918 verts[i].uv[0] = cube_vertices_textured[i * 9 + 7] * textureScaling;
919 verts[i].uv[1] = cube_vertices_textured[i * 9 + 8] * textureScaling;
920 }
921
922 glmesh->m_numIndices = numIndices;
923 glmesh->m_numvertices = numVertices;
924 break;
925 }
926
927 case URDF_GEOM_SPHERE:
928 {
929 btScalar radius = visual->m_geometry.m_sphereRadius;
930 btSphereShape* sphereShape = new btSphereShape(radius);
931 convexColShape = sphereShape;
932 convexColShape->setMargin(gUrdfDefaultCollisionMargin);
933 break;
934 }
935
936 case URDF_GEOM_MESH:
937 {
938 switch (visual->m_geometry.m_meshFileType)
939 {
940 case UrdfGeometry::FILE_OBJ:
941 {
942
943 if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO))
944 {
945 if (meshData.m_textureImage1)
946 {
947 BulletURDFTexture texData;
948 texData.m_width = meshData.m_textureWidth;
949 texData.m_height = meshData.m_textureHeight;
950 texData.textureData1 = meshData.m_textureImage1;
951 texData.m_isCached = meshData.m_isCached;
952 texturesOut.push_back(texData);
953 }
954 glmesh = meshData.m_gfxShape;
955 }
956 break;
957 }
958
959 case UrdfGeometry::FILE_STL:
960 {
961 glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str(),m_data->m_fileIO);
962 break;
963 }
964
965 case UrdfGeometry::FILE_COLLADA:
966 {
967 btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
968 btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
969 btTransform upAxisTrans;
970 upAxisTrans.setIdentity();
971 float unitMeterScaling = 1;
972 int upAxis = 2;
973
974 LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str(),
975 visualShapes,
976 visualShapeInstances,
977 upAxisTrans,
978 unitMeterScaling,
979 upAxis,
980 m_data->m_fileIO);
981
982 glmesh = new GLInstanceGraphicsShape;
983 // int index = 0;
984 glmesh->m_indices = new b3AlignedObjectArray<int>();
985 glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
986
987 for (int i = 0; i < visualShapeInstances.size(); i++)
988 {
989 ColladaGraphicsInstance* instance = &visualShapeInstances[i];
990 GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
991
992 b3AlignedObjectArray<GLInstanceVertex> verts;
993 verts.resize(gfxShape->m_vertices->size());
994
995 int baseIndex = glmesh->m_vertices->size();
996
997 for (int i = 0; i < gfxShape->m_vertices->size(); i++)
998 {
999 verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
1000 verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
1001 verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
1002 verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
1003 verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
1004 verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
1005 verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
1006 verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
1007 verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
1008 }
1009
1010 int curNumIndices = glmesh->m_indices->size();
1011 int additionalIndices = gfxShape->m_indices->size();
1012 glmesh->m_indices->resize(curNumIndices + additionalIndices);
1013 for (int k = 0; k < additionalIndices; k++)
1014 {
1015 glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
1016 }
1017
1018 //compensate upAxisTrans and unitMeterScaling here
1019 btMatrix4x4 upAxisMat;
1020 upAxisMat.setIdentity();
1021 // upAxisMat.setPureRotation(upAxisTrans.getRotation());
1022 btMatrix4x4 unitMeterScalingMat;
1023 unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
1024 btMatrix4x4 worldMat = unitMeterScalingMat * upAxisMat * instance->m_worldTransform;
1025 //btMatrix4x4 worldMat = instance->m_worldTransform;
1026 int curNumVertices = glmesh->m_vertices->size();
1027 int additionalVertices = verts.size();
1028 glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
1029
1030 for (int v = 0; v < verts.size(); v++)
1031 {
1032 btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
1033 pos = worldMat * pos;
1034 verts[v].xyzw[0] = float(pos[0]);
1035 verts[v].xyzw[1] = float(pos[1]);
1036 verts[v].xyzw[2] = float(pos[2]);
1037 glmesh->m_vertices->push_back(verts[v]);
1038 }
1039 }
1040 glmesh->m_numIndices = glmesh->m_indices->size();
1041 glmesh->m_numvertices = glmesh->m_vertices->size();
1042 //glmesh = LoadMeshFromCollada(visual->m_geometry.m_meshFileName);
1043
1044 break;
1045 }
1046 } // switch file type
1047
1048 if (!glmesh || !glmesh->m_vertices || glmesh->m_numvertices <= 0)
1049 {
1050 b3Warning("%s: cannot extract anything useful from mesh '%s'\n", urdfPathPrefix, visual->m_geometry.m_meshFileName.c_str());
1051 break;
1052 }
1053
1054 //apply the geometry scaling
1055 for (int i = 0; i < glmesh->m_vertices->size(); i++)
1056 {
1057 glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0];
1058 glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1];
1059 glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2];
1060 }
1061 break;
1062 }
1063 case URDF_GEOM_PLANE:
1064 {
1065 b3Warning("No default visual for URDF_GEOM_PLANE");
1066 break;
1067 }
1068 default:
1069 {
1070 b3Warning("Error: unknown visual geometry type %i\n", visual->m_geometry.m_type);
1071 }
1072 }
1073
1074 //if we have a convex, tesselate into localVertices/localIndices
1075 if ((glmesh == 0) && convexColShape)
1076 {
1077 BT_PROFILE("convexColShape");
1078
1079 btShapeHull* hull = new btShapeHull(convexColShape);
1080 hull->buildHull(0.0);
1081 {
1082 // int strideInBytes = 9*sizeof(float);
1083 int numVertices = hull->numVertices();
1084 int numIndices = hull->numIndices();
1085
1086 glmesh = new GLInstanceGraphicsShape;
1087 // int index = 0;
1088 glmesh->m_indices = new b3AlignedObjectArray<int>();
1089 glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
1090
1091 for (int i = 0; i < numVertices; i++)
1092 {
1093 GLInstanceVertex vtx;
1094 btVector3 pos = hull->getVertexPointer()[i];
1095 vtx.xyzw[0] = pos.x();
1096 vtx.xyzw[1] = pos.y();
1097 vtx.xyzw[2] = pos.z();
1098 vtx.xyzw[3] = 1.f;
1099 pos.normalize();
1100 vtx.normal[0] = pos.x();
1101 vtx.normal[1] = pos.y();
1102 vtx.normal[2] = pos.z();
1103 btScalar u = btAtan2(vtx.normal[0], vtx.normal[2]) / (2 * SIMD_PI) + 0.5;
1104 btScalar v = vtx.normal[1] * 0.5 + 0.5;
1105 vtx.uv[0] = u;
1106 vtx.uv[1] = v;
1107 glmesh->m_vertices->push_back(vtx);
1108 }
1109
1110 btAlignedObjectArray<int> indices;
1111 for (int i = 0; i < numIndices; i++)
1112 {
1113 glmesh->m_indices->push_back(hull->getIndexPointer()[i]);
1114 }
1115
1116 glmesh->m_numvertices = glmesh->m_vertices->size();
1117 glmesh->m_numIndices = glmesh->m_indices->size();
1118 }
1119 delete hull;
1120 delete convexColShape;
1121 convexColShape = 0;
1122 }
1123
1124 if (glmesh && glmesh->m_numIndices > 0 && glmesh->m_numvertices > 0)
1125 {
1126 BT_PROFILE("glmesh");
1127 int baseIndex = verticesOut.size();
1128
1129 for (int i = 0; i < glmesh->m_indices->size(); i++)
1130 {
1131 indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex);
1132 }
1133
1134 for (int i = 0; i < glmesh->m_vertices->size(); i++)
1135 {
1136 GLInstanceVertex& v = glmesh->m_vertices->at(i);
1137 btVector3 vert(v.xyzw[0], v.xyzw[1], v.xyzw[2]);
1138 btVector3 vt = visualTransform * vert;
1139 v.xyzw[0] = vt[0];
1140 v.xyzw[1] = vt[1];
1141 v.xyzw[2] = vt[2];
1142 btVector3 triNormal(v.normal[0], v.normal[1], v.normal[2]);
1143 triNormal = visualTransform.getBasis() * triNormal;
1144 v.normal[0] = triNormal[0];
1145 v.normal[1] = triNormal[1];
1146 v.normal[2] = triNormal[2];
1147 verticesOut.push_back(v);
1148 }
1149 }
1150 delete glmesh;
1151 }
1152
1153 int PhysXURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
1154 {
1155 int graphicsIndex = -1;
1156 btAlignedObjectArray<GLInstanceVertex> vertices;
1157 btAlignedObjectArray<int> indices;
1158 btTransform startTrans;
1159 startTrans.setIdentity();
1160 btAlignedObjectArray<PhysXURDFTexture> textures;
1161
1162 const UrdfModel& model = m_data->m_urdfParser.getModel();
1163 UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
1164 if (linkPtr)
1165 {
1166 const UrdfLink* link = *linkPtr;
1167
1168 for (int v = 0; v < link->m_visualArray.size(); v++)
1169 {
1170 const UrdfVisual& vis = link->m_visualArray[v];
1171 btTransform childTrans = vis.m_linkLocalFrame;
1172 btHashString matName(vis.m_materialName.c_str());
1173 UrdfMaterial* const* matPtr = model.m_materials[matName];
1174 b3ImportMeshData meshData;
1175
1176 convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse() * childTrans, vertices, indices, textures,meshData);
1177
1178 if (m_data->m_flags&CUF_USE_MATERIAL_COLORS_FROM_MTL)
1179 {
1180 if ((meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) &&
1181 (meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR))
1182 {
1183 UrdfMaterialColor matCol;
1184
1185 if (m_data->m_flags&CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL)
1186 {
1187 matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0],
1188 meshData.m_rgbaColor[1],
1189 meshData.m_rgbaColor[2],
1190 meshData.m_rgbaColor[3]);
1191 } else
1192 {
1193 matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0],
1194 meshData.m_rgbaColor[1],
1195 meshData.m_rgbaColor[2],
1196 1);
1197 }
1198
1199 matCol.m_specularColor.setValue(meshData.m_specularColor[0],
1200 meshData.m_specularColor[1],
1201 meshData.m_specularColor[2]);
1202 m_data->m_linkColors.insert(linkIndex, matCol);
1203 }
1204 } else
1205 {
1206 if (matPtr)
1207 {
1208 UrdfMaterial* const mat = *matPtr;
1209 //printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
1210 UrdfMaterialColor matCol;
1211 matCol.m_rgbaColor = mat->m_matColor.m_rgbaColor;
1212 matCol.m_specularColor = mat->m_matColor.m_specularColor;
1213 m_data->m_linkColors.insert(linkIndex, matCol);
1214 }
1215 }
1216
1217 }
1218 }
1219 if (vertices.size() && indices.size())
1220 {
1221 // graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
1222 //graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
1223
1224 //CommonRenderInterface* renderer = m_data->m_guiHelper->getRenderInterface();
1225
1226 if (1)
1227 {
1228 int textureIndex = -1;
1229 if (textures.size())
1230 {
1231 textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1, textures[0].m_width, textures[0].m_height);
1232 if (textureIndex >= 0)
1233 {
1234 m_data->m_allocatedTextures.push_back(textureIndex);
1235 }
1236 }
1237 {
1238 B3_PROFILE("registerGraphicsShape");
1239 graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex);
1240 }
1241 }
1242 }
1243
1244 //delete textures
1245 for (int i = 0; i < textures.size(); i++)
1246 {
1247 B3_PROFILE("free textureData");
1248 if (!textures[i].m_isCached)
1249 {
1250 free(textures[i].textureData1);
1251 }
1252 }
1253 return graphicsIndex;
1254 }
1255 #endif
getLinkColor(int linkIndex,btVector4 & colorRGBA) const1256 bool PhysXURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
1257 {
1258 const UrdfMaterialColor* matColPtr = m_data->m_linkColors[linkIndex];
1259 if (matColPtr)
1260 {
1261 colorRGBA = matColPtr->m_rgbaColor;
1262 return true;
1263 }
1264 return false;
1265 }
1266
getLinkColor2(int linkIndex,UrdfMaterialColor & matCol) const1267 bool PhysXURDFImporter::getLinkColor2(int linkIndex, UrdfMaterialColor& matCol) const
1268 {
1269 UrdfMaterialColor* matColPtr = m_data->m_linkColors[linkIndex];
1270 if (matColPtr)
1271 {
1272 matCol = *matColPtr;
1273 return true;
1274 }
1275 return false;
1276 }
1277
setLinkColor2(int linkIndex,struct UrdfMaterialColor & matCol) const1278 void PhysXURDFImporter::setLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const
1279 {
1280 m_data->m_linkColors.insert(linkIndex, matCol);
1281 }
1282
getLinkContactInfo(int urdflinkIndex,URDFLinkContactInfo & contactInfo) const1283 bool PhysXURDFImporter::getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo) const
1284 {
1285 UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdflinkIndex);
1286 if (linkPtr)
1287 {
1288 const UrdfLink* link = *linkPtr;
1289 contactInfo = link->m_contactInfo;
1290 return true;
1291 }
1292 return false;
1293 }
1294
getLinkAudioSource(int linkIndex,SDFAudioSource & audioSource) const1295 bool PhysXURDFImporter::getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const
1296 {
1297 UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
1298 if (linkPtr)
1299 {
1300 const UrdfLink* link = *linkPtr;
1301 if (link->m_audioSource.m_flags & SDFAudioSource::SDFAudioSourceValid)
1302 {
1303 audioSource = link->m_audioSource;
1304 return true;
1305 }
1306 }
1307 return false;
1308 }
1309
setEnableTinyRenderer(bool enable)1310 void PhysXURDFImporter::setEnableTinyRenderer(bool enable)
1311 {
1312 m_data->m_enableTinyRenderer = enable;
1313 }
1314
1315
1316
convertLinkVisualShapes3(int linkIndex,const char * pathPrefix,const btTransform & localInertiaFrame,const UrdfLink * linkPtr,const UrdfModel * model,int collisionObjectUniqueId,int bodyUniqueId,struct CommonFileIOInterface * fileIO) const1317 int PhysXURDFImporter::convertLinkVisualShapes3(
1318 int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
1319 const UrdfLink* linkPtr, const UrdfModel* model,
1320 int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) const
1321 {
1322 return 0;
1323 }
1324
convertLinkVisualShapes2(int linkIndex,int urdfIndex,const char * pathPrefix,const btTransform & localInertiaFrame,class btCollisionObject * colObj,int bodyUniqueId) const1325 void PhysXURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
1326 {
1327 if (m_data->m_enableTinyRenderer && m_data->m_customVisualShapesConverter)
1328 {
1329 const UrdfModel& model = m_data->m_urdfParser.getModel();
1330 UrdfLink* const* linkPtr = model.m_links.getAtIndex(urdfIndex);
1331 if (linkPtr)
1332 {
1333 m_data->m_customVisualShapesConverter->setFlags(m_data->m_flags);
1334 m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, 0, bodyUniqueId, m_data->m_fileIO);
1335 }
1336 }
1337 }
1338
getNumAllocatedCollisionShapes() const1339 int PhysXURDFImporter::getNumAllocatedCollisionShapes() const
1340 {
1341 return m_data->m_allocatedCollisionShapes.size();
1342 }
1343
getAllocatedCollisionShape(int index)1344 btCollisionShape* PhysXURDFImporter::getAllocatedCollisionShape(int index)
1345 {
1346 return m_data->m_allocatedCollisionShapes[index];
1347 }
1348
getNumAllocatedMeshInterfaces() const1349 int PhysXURDFImporter::getNumAllocatedMeshInterfaces() const
1350 {
1351 return 0;// m_data->m_allocatedMeshInterfaces.size();
1352 }
1353
getAllocatedMeshInterface(int index)1354 btStridingMeshInterface* PhysXURDFImporter::getAllocatedMeshInterface(int index)
1355 {
1356 return 0;// m_data->m_allocatedMeshInterfaces[index];
1357 }
1358
getNumAllocatedTextures() const1359 int PhysXURDFImporter::getNumAllocatedTextures() const
1360 {
1361 return m_data->m_allocatedTextures.size();
1362 }
1363
getAllocatedTexture(int index) const1364 int PhysXURDFImporter::getAllocatedTexture(int index) const
1365 {
1366 return m_data->m_allocatedTextures[index];
1367 }
1368
getCollisionGroupAndMask(int linkIndex,int & colGroup,int & colMask) const1369 int PhysXURDFImporter::getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const
1370 {
1371 int result = 0;
1372 UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
1373 btAssert(linkPtr);
1374 if (linkPtr)
1375 {
1376 UrdfLink* link = *linkPtr;
1377 for (int v = 0; v < link->m_collisionArray.size(); v++)
1378 {
1379 const UrdfCollision& col = link->m_collisionArray[v];
1380 if (col.m_flags & URDF_HAS_COLLISION_GROUP)
1381 {
1382 colGroup = col.m_collisionGroup;
1383 result |= URDF_HAS_COLLISION_GROUP;
1384 }
1385 if (col.m_flags & URDF_HAS_COLLISION_MASK)
1386 {
1387 colMask = col.m_collisionMask;
1388 result |= URDF_HAS_COLLISION_MASK;
1389 }
1390 }
1391 }
1392 return result;
1393 }
1394
1395 #if 0
1396 class btCompoundShape* PhysXURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
1397 {
1398
1399 btCompoundShape* compoundShape = new btCompoundShape();
1400 m_data->m_allocatedCollisionShapes.push_back(compoundShape);
1401
1402 compoundShape->setMargin(gUrdfDefaultCollisionMargin);
1403 UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
1404 btAssert(linkPtr);
1405 if (linkPtr)
1406 {
1407 UrdfLink* link = *linkPtr;
1408
1409 for (int v = 0; v < link->m_collisionArray.size(); v++)
1410 {
1411 const UrdfCollision& col = link->m_collisionArray[v];
1412 btCollisionShape* childShape = convertURDFToCollisionShape(&col, pathPrefix);
1413 if (childShape)
1414 {
1415 m_data->m_allocatedCollisionShapes.push_back(childShape);
1416 if (childShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
1417 {
1418 btCompoundShape* compound = (btCompoundShape*)childShape;
1419 for (int i = 0; i < compound->getNumChildShapes(); i++)
1420 {
1421 m_data->m_allocatedCollisionShapes.push_back(compound->getChildShape(i));
1422 }
1423 }
1424
1425 btTransform childTrans = col.m_linkLocalFrame;
1426
1427 compoundShape->addChildShape(localInertiaFrame.inverse() * childTrans, childShape);
1428 }
1429 }
1430 }
1431
1432 return compoundShape;
1433 }
1434 #endif
1435