diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
index 53e66202d7fa874a6d41d14023cf8d80feb0bc97..41cd623299606278356805e5933c4afc57fc7d4c 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
@@ -11,11 +11,14 @@
#include <BulletCollision/CollisionShapes/btShapeHull.h>
#include <BulletCollision/CollisionShapes/btBoxShape.h>
+#include <BulletCollision/CollisionShapes/btSphereShape.h>
#include <BulletCollision/CollisionShapes/btCompoundShape.h>
//#define DEBUG_FIXED_OBJECTS
//#define USE_BULLET_GENERIC_6DOF_CONSTRAINT
+#include <typeinfo>
+
using namespace VirtualRobot;
namespace SimDynamics {
@@ -33,7 +36,7 @@ BulletObject::BulletObject(VirtualRobot::SceneObjectPtr o)
THROW_VR_EXCEPTION_IF(!o,"NULL object");
CollisionModelPtr colModel = o->getCollisionModel();
- if (!colModel)
+ if (!colModel)
{
VR_WARNING << "Building empty collision shape for object " << o->getName() << endl;
collisionShape.reset(new btEmptyShape());
@@ -42,16 +45,34 @@ BulletObject::BulletObject(VirtualRobot::SceneObjectPtr o)
VirtualRobot::ObstaclePtr ob = Obstacle::createBox(10.0f,10.0f,10.0f);
ob->setGlobalPose(o->getGlobalPose());
colModel = ob->getCollisionModel();*/
- } else
+ } else
{
- THROW_VR_EXCEPTION_IF(!colModel,"No CollisionModel, could not create dynamics model...");
+ THROW_VR_EXCEPTION_IF(!colModel, "No CollisionModel, could not create dynamics model...");
if (o->getName() != "Floor")
{
- TriMeshModelPtr trimesh;
- trimesh = colModel->getTriMeshModel();
- THROW_VR_EXCEPTION_IF( ( !trimesh || trimesh->faces.size()==0) , "No TriMeshModel, could not create dynamics model...");
- collisionShape.reset(createConvexHullShape(trimesh));
+
+ std::vector<VisualizationNode::PrimitivePtr> primitives = colModel->getVisualization()->primitives;
+ if (primitives.size() == 1)
+ {
+ collisionShape.reset(getShapeFromPrimitive(primitives[0]));
+ }
+ else if (primitives.size() > 1)
+ {
+ std::vector<VisualizationNode::PrimitivePtr>::iterator it;
+ btCompoundShape *compoundShape = new btCompoundShape();
+ for (it = primitives.begin(); it != primitives.end(); it++) {
+ compoundShape->addChildShape(BulletEngine::getPoseBullet((*it)->transform), getShapeFromPrimitive(*it));
+ }
+ collisionShape.reset(compoundShape);
+ }
+ else
+ {
+ TriMeshModelPtr trimesh;
+ trimesh = colModel->getTriMeshModel();
+ THROW_VR_EXCEPTION_IF( ( !trimesh || trimesh->faces.size()==0) , "No TriMeshModel, could not create dynamics model...");
+ collisionShape.reset(createConvexHullShape(trimesh));
+ }
}
else
{
@@ -129,6 +150,30 @@ BulletObject::~BulletObject()
delete motionState;
}
+
+btCollisionShape* BulletObject::getShapeFromPrimitive(VirtualRobot::VisualizationNode::PrimitivePtr primitive)
+{
+ btCollisionShape* result;
+ if (primitive->type == VisualizationNode::Box::TYPE)
+ {
+ VisualizationNode::Box* box = boost::dynamic_pointer_cast<VisualizationNode::Box>(primitive).get();
+ btBoxShape *boxShape = new btBoxShape(btVector3(box->width / 1000.f, box->height / 1000.f, box->depth / 1000.f));
+ result = boxShape;
+ }
+ else if (primitive->type == VisualizationNode::Sphere::TYPE)
+ {
+ VisualizationNode::Sphere* sphere = boost::dynamic_pointer_cast<VisualizationNode::Sphere>(primitive).get();
+ btSphereShape *sphereShape = new btSphereShape(btScalar(sphere->radius / 1000.f));
+ result = sphereShape;
+ }
+ else
+ {
+ VR_ERROR << "Unsupported shape type " << primitive->type << std::endl;
+ result = new btEmptyShape();
+ }
+ return result;
+}
+
btConvexHullShape* BulletObject::createConvexHullShape(VirtualRobot::TriMeshModelPtr trimesh)
{
VR_ASSERT(trimesh);
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
index 99d0cc95617d8b6c1ffad71d91fb2d1058937894..79a70048c652f6c3ca527b8c7564f61892f13056 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
@@ -84,11 +84,12 @@ public:
protected:
void setPoseIntern(const Eigen::Matrix4f &pose);
+ btCollisionShape* getShapeFromPrimitive(VirtualRobot::VisualizationNode::PrimitivePtr primitive);
btConvexHullShape* createConvexHullShape(VirtualRobot::TriMeshModelPtr trimesh);
boost::shared_ptr<btRigidBody> rigidBody;
- boost::shared_ptr<btCollisionShape> collisionShape; // bullet collision shape
+ boost::shared_ptr<btCollisionShape> collisionShape; // bullet collision shape
Eigen::Vector3f com; // com offset of trimesh
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
index c71c0ad8226f142f5434705d4312333e9a7f9170..9ce1e4812b800efe94b4fb054681c8cd07e58132 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
@@ -66,6 +66,21 @@ SimDynamicsWindow::SimDynamicsWindow(std::string &sRobotFilename, Qt::WFlags fla
dynamicsObject->setPosition(Eigen::Vector3f(1000,2000,1000.0f));
dynamicsWorld->addObject(dynamicsObject);
+ ManipulationObjectPtr vitalis;
+ std::string vitalisPath = "objects/VitalisWithPrimitives.xml";
+ if (VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(vitalisPath))
+ {
+ vitalis = ObjectIO::loadManipulationObject(vitalisPath);
+ }
+ if (vitalis)
+ {
+ vitalis->setMass(0.5f);
+ dynamicsObjectVitalis = dynamicsWorld->CreateDynamicsObject(vitalis);
+ dynamicsObjectVitalis->setPosition(Eigen::Vector3f(-1000.f, 2000.f, 1000.f));
+ dynamicsWorld->addObject(dynamicsObjectVitalis);
+ }
+
+
#if 0
std::string f = "/home/niko/coding/armarx/SimulationX/data/environment/KIT_Robot_Kitchen.xml";
ManipulationObjectPtr mo = ObjectIO::loadManipulationObject(f);
@@ -193,6 +208,8 @@ void SimDynamicsWindow::buildVisualization()
SceneObject::VisualizationType colModel = useColModel?SceneObject::Collision:SceneObject::Full;
viewer->addVisualization(dynamicsRobot,colModel);
viewer->addVisualization(dynamicsObject,colModel);
+ if (dynamicsObjectVitalis)
+ viewer->addVisualization(dynamicsObjectVitalis,colModel);
if (dynamicsObject2)
viewer->addVisualization(dynamicsObject2,colModel);
}
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
index 2ceafa1731328c091cdd2e979b2ef85d9868a7c1..4a52c93c2955fa7a024419efad96024bbc25cb33 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
@@ -75,6 +75,7 @@ protected:
SimDynamics::DynamicsRobotPtr dynamicsRobot;
SimDynamics::DynamicsObjectPtr dynamicsObject;
SimDynamics::DynamicsObjectPtr dynamicsObject2;
+ SimDynamics::DynamicsObjectPtr dynamicsObjectVitalis;
Ui::MainWindowBulletViewer UI;
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp
index 4204acf3dad4223f6370c410058a04b004a683bc..4e93095f217c58d05d3afa6962190bbb385b8e04 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.cpp
@@ -37,7 +37,7 @@ CoinVisualizationNode::CoinVisualizationNode(SoNode* visualizationNode) :
visualizationAtGlobalPose->ref();
globalPoseTransform = new SoMatrixTransform();
- visualizationAtGlobalPose->addChild(globalPoseTransform);
+ visualizationAtGlobalPose->addChild(globalPoseTransform);
attachedVisualizationsSeparator = new SoSeparator();
attachedVisualizationsSeparator->ref();
@@ -243,7 +243,7 @@ void CoinVisualizationNode::attachVisualization(const std::string &name, Visuali
if (coinVisualizationNode && coinVisualizationNode->getCoinVisualization())
{
attachedCoinVisualizations[name] = coinVisualizationNode->getCoinVisualization();
- attachedVisualizationsSeparator->addChild(coinVisualizationNode->getCoinVisualization());
+ attachedVisualizationsSeparator->addChild(coinVisualizationNode->getCoinVisualization());
}
}
diff --git a/VirtualRobot/Visualization/VisualizationNode.cpp b/VirtualRobot/Visualization/VisualizationNode.cpp
index 6934be4a933c16f1fb96dddd52640aa43412e1fc..206a5e8584337f525315ac7934d5c49d6acb7cfa 100644
--- a/VirtualRobot/Visualization/VisualizationNode.cpp
+++ b/VirtualRobot/Visualization/VisualizationNode.cpp
@@ -204,5 +204,4 @@ void VisualizationNode::scale( Eigen::Vector3f &scaleFactor )
VR_WARNING << "not implemented..." << endl;
}
-
} // namespace VirtualRobot
diff --git a/VirtualRobot/Visualization/VisualizationNode.h b/VirtualRobot/Visualization/VisualizationNode.h
index e7ae7b34fc2c2c284347d6eb50f24e80a489a940..5bd62722c5670eb0f60ac1cd04c81ce8bf551d33 100644
--- a/VirtualRobot/Visualization/VisualizationNode.h
+++ b/VirtualRobot/Visualization/VisualizationNode.h
@@ -39,6 +39,34 @@ class VIRTUAL_ROBOT_IMPORT_EXPORT VisualizationNode
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ struct Primitive {
+ static const int TYPE = 0;
+ Primitive() : type(TYPE), transform(Eigen::Matrix4f::Identity()) {}
+ virtual ~Primitive() {} //needed for dynamic cast to work
+ Eigen::Matrix4f transform;
+ const int type;
+ protected:
+ Primitive(int type) : type(type) {}
+ };
+
+ struct Box : public Primitive {
+ static const int TYPE = 1;
+ Box() : Primitive(TYPE) {}
+ float width;
+ float height;
+ float depth;
+ };
+
+ struct Sphere : public Primitive {
+ static const int TYPE = 2;
+ Sphere() : Primitive(TYPE) {}
+ float radius;
+ };
+
+ typedef boost::shared_ptr<Primitive> PrimitivePtr;
+
+ std::vector<PrimitivePtr> primitives;
+
/*!
Constructor
*/
diff --git a/VirtualRobot/XML/BaseIO.cpp b/VirtualRobot/XML/BaseIO.cpp
index 0b58727c262ea6db36009820c08ad288897279ce..37025ac1802f61b09e1aa1159404de2b5ba0a8a0 100644
--- a/VirtualRobot/XML/BaseIO.cpp
+++ b/VirtualRobot/XML/BaseIO.cpp
@@ -712,6 +712,7 @@ VisualizationNodePtr BaseIO::processVisualizationTag(rapidxml::xml_node<char> *v
std::string visuFileType = "";
std::string visuFile = "";
rapidxml::xml_attribute<> *attr;
+ std::vector<VisualizationNode::PrimitivePtr> primitives;
VisualizationNodePtr visualizationNode;
if (!visuXMLNode)
@@ -749,14 +750,68 @@ VisualizationNodePtr BaseIO::processVisualizationTag(rapidxml::xml_node<char> *v
//visuFile = visuFileXMLNode->value();
//makeAbsolutePath(basePath,visuFile);
}
- if (visuFile!="")
+
+ rapidxml::xml_node<> *visuPrimitivesXMLNode = visuXMLNode->first_node("primitives",0,false);
+ if (visuPrimitivesXMLNode)
+ {
+ rapidxml::xml_node<> *primitiveXMLNode = visuPrimitivesXMLNode->first_node();
+
+ while (primitiveXMLNode)
+ {
+ //extract info
+ std::string pName = primitiveXMLNode->name();
+ VisualizationNode::PrimitivePtr primitive;
+
+ float lenFactor = 1.f;
+ if (hasUnitsAttribute(primitiveXMLNode))
+ {
+ Units u = getUnitsAttribute(primitiveXMLNode,Units::eLength);
+ if (u.isMeter())
+ {
+ lenFactor = 1000.f;
+ }
+ }
+
+ if (pName == "Box")
+ {
+ VisualizationNode::Box *box = new VisualizationNode::Box;
+ box->width = convertToFloat(primitiveXMLNode->first_attribute("width",0,false)->value()) * lenFactor;
+ box->height = convertToFloat(primitiveXMLNode->first_attribute("height",0,false)->value()) * lenFactor;
+ box->depth = convertToFloat(primitiveXMLNode->first_attribute("depth",0,false)->value()) * lenFactor;
+ primitive.reset(box);
+ } else if (pName == "Sphere")
+ {
+ VisualizationNode::Sphere *sphere = new VisualizationNode::Sphere;
+ sphere->radius = convertToFloat(primitiveXMLNode->first_attribute("radius",0,false)->value()) * lenFactor;
+ primitive.reset(sphere);
+ } else
+ {
+ VR_ERROR << "Unknown primitive type \"" << pName << "\"; skipping";
+ }
+
+ if (primitive)
+ {
+ Eigen::Matrix4f transform;
+ processTransformNode(primitiveXMLNode->first_node("transform",0,false), "transform", transform);
+ primitive->transform = transform;
+ primitives.push_back(primitive);
+ }
+
+ primitiveXMLNode = primitiveXMLNode->next_sibling();
+ }
+ }
+
+ if (visuFile!="")
{
VisualizationFactoryPtr visualizationFactory = VisualizationFactory::fromName(visuFileType, NULL);
- if (visualizationFactory)
- visualizationNode = visualizationFactory->getVisualizationFromFile(visuFile);
- else
+ if (visualizationFactory)
+ {
+ visualizationNode = visualizationFactory->getVisualizationFromFile(visuFile);
+ visualizationNode->primitives = primitives;
+ } else
VR_WARNING << "VisualizationFactory of type '" << visuFileType << "' not present. Ignoring Visualization data in Node <" << tagName << ">" << endl;
- }
+ }
+
rapidxml::xml_node<> *coordXMLNode = visuXMLNode->first_node("coordinateaxis",0,false);
if (coordXMLNode)
@@ -862,8 +917,8 @@ CollisionModelPtr BaseIO::processCollisionTag(rapidxml::xml_node<char> *colXMLNo
{
bbox = isTrue(attr->value());
}
-
}
+
if (collisionFile!="")
{
VisualizationFactoryPtr visualizationFactory = VisualizationFactory::fromName(collisionFileType, NULL);
diff --git a/VirtualRobot/XML/ObjectIO.cpp b/VirtualRobot/XML/ObjectIO.cpp
index 1cc246a314037778527d0b24b06237646a183696..0200ff23d7faa279e90188d28776dcde6d553062 100644
--- a/VirtualRobot/XML/ObjectIO.cpp
+++ b/VirtualRobot/XML/ObjectIO.cpp
@@ -231,6 +231,8 @@ ManipulationObjectPtr ObjectIO::processManipulationObject(rapidxml::xml_node<cha
{
THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in ManipulationObject '" << objName << "'." << endl);
collisionModel = processCollisionTag(node, objName, basePath);
+ if (collisionModel && visualizationNode)
+ collisionModel->getVisualization()->primitives = visualizationNode->primitives;
colProcessed = true;
} else if (nodeName == "physics")
{
@@ -327,6 +329,8 @@ ObstaclePtr ObjectIO::processObstacle(rapidxml::xml_node<char>* objectXMLNode, c
{
THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << objName << "'." << endl);
collisionModel = processCollisionTag(node, objName, basePath);
+ if (collisionModel && visualizationNode)
+ collisionModel->getVisualization()->primitives = visualizationNode->primitives;
colProcessed = true;
} else if (nodeName == "physics")
{